ROSで簡単なc++パッケージを作成する(初心者向け)

ROSの勉強を始めたのですが、公式のチュートリアルや本は序盤説明が多く、手を動かしながら覚えるタイプの自分としてはなかなか覚えられませんでした。
そのため、単純なデータを送受信するノードを含んだパッケージの作成方法を簡単にまとめてみました。
ROSを始める人の参考になれば幸いです。
(私も初心者なため、誤りがあればご指摘ください)

なお、環境構築については説明しません。環境構築は[公式のチュートリアル](http://wiki.ros.org/ja/ROS/Tutorials/InstallingandConfiguringROSEnvironment)などを参照してください。
また、以下の説明ではワークスペースを`~/catkin_ws`に構築したとして説明します。

# 環境
ubuntu 16.04
ROSバージョン Kinetic

# 今回作成するもの  
|                     | 説明                         |
|:--------------------|:----------------------------|
| sample_c_publisher  | データを送信するノード         |
| sample_c_subscriber | データを受信するノード         |
| sample_message      | ノード間でやりとりするメッセージ |

![fig.png](https://qiita-image-store.s3.amazonaws.com/0/293692/6804da74-82e1-cc94-5cce-6143ad3c4097.png)

ROSの通信方法にはメッセージとサービスがありますが、今回はメッセージを作成します。
また、今回作成するファイルおよび編集するファイルは以下になります。(自動生成されるファイルの一部は省略しています)

```
catkin_ws
`- src
   `- sample_c
      |- msg
      |  `- sample_message.msg
      |- src
      |  |- sample_c_publisher.cpp
      |  `- sample_c_subscriber.cpp
      |- package.xml
      `- CMakeLists.txt
```

# パッケージを作成する
パッケージを作成するには以下のコマンドを使用します。

```
$ catkin_create_pkg <package_name> [depend1] [depend2] [depend3] ...
```
今回は`sample_c`というパッケージを作成します。  

```
$ cd ~/catkin_ws/src
$ catkin_create_pkg sample_c roscpp std_msgs
```
このコマンドを実行すると、`sample_c`というディレクトリとその中に`package.xml`、`CMakeLists.txt`が生成されます

# メッセージを作成する
メッセージファイルを作成します。  

```
$ cd ~/catkin_ws/src/sample_c
$ mkdir msg
$ cd msg
$ touch sample_message.msg
```
作成したファイルを編集し内部に含むデータを定義します。今回はstringと32byte unsigned intを含むこととします。  

```sample_message.msg
string message
uint32 count
```

このファイルよりメッセージを定義したヘッダファイルをする必要があります。そのために、`CMakeLists.txt`を編集します。
`CMakeLists.txt`を開くと、様々な設定が記載されており一部がコメントアウトされています。今回は以下の箇所だけ編集すればよいです。  

```cmake:CMakeLists.txt

find_package(catkin REQUIRED COMPONENTS
   roscpp
   std_msgs
   message_generation
)

add_message_files(
  FILES
  sample_message.msg
)

generate_messages(
  DEPENDENCIES
  std_msgs
)

catkin_package(
  CATKIN_DEPENDS message_runtime roscpp std_msgs
)
```
次に`package.xml`を開いて編集します。以下2行がコメントアウトされているのでコメントタグを外します。  

```package.xml
<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>
```
最後にビルドし、ヘッダファイルを生成します。  

```
$ cd ~/catkin_ws
$ catkin_make
```
ビルドが成功すると`catkin_ws/devel/include/sample_c/sample_message.h`というファイルが生成されます。

# 送信側(publisher)を作成する  
ファイルを生成し、以下のように実装します。

```
$ cd ~/catkin_ws/src/sample_c
$ mkdir src
$ cd src
$ touch sample_c_publisher.cpp
```
```c++:sample_c_publisher.cpp
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "sample_c/sample_message.h"  // 生成したメッセージのヘッダファイル
#include <sstream>

int main(int argc, char **argv) {
  // 初期化し、ノードの名前を"sample_c_publisher"とする
  ros::init(argc, argv, "sample_c_publisher");
  ros::NodeHandle n;
  // メッセージを"sample_topic"というトピックに送信する
  ros::Publisher publisher = n.advertise<sample_c::sample_message>("sample_topic", 1000);
  // 1秒間に2回データを送信する
  ros::Rate loop_rate(2);

  int count = 0;
  while (ros::ok()) {
    std::stringstream ss;
    ss << "hello world " << count;
    ROS_INFO("message = %s, count = %d", ss.str().c_str(), count);

    // 送信するメッセージの作成
    sample_c::sample_message msg;
    msg.message = ss.str();
    msg.count = count;

    // 送信
    publisher.publish(msg);

    ros::spinOnce();

    loop_rate.sleep();
    ++count;
  }

  return 0;
}
```

# 受信側(subscriber)を作成する
ファイルを生成し、以下のように実装します。

```
$ cd ~/catkin_ws/src/sample_c/src
$ touch sample_c_subscriber.cpp
```
```c++:sample_c_subscriber.cpp
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "sample_c/sample_message.h"

void chatterCallback(const sample_c::sample_message &msg) {
  // 受信したデータを出力する
  ROS_INFO("I heard: message = [%s], count = [%d]", msg.message.c_str(), msg.count);
}

int main(int argc, char **argv) {
  // 初期化し、ノードの名前を"sample_c_subscriber"とする
  ros::init(argc, argv, "sample_c_subscriber");
  ros::NodeHandle n;
  // "sample_topic"というトピックからデータを受信する
  ros::Subscriber subscriber = n.subscribe("sample_topic", 1000, chatterCallback);

  ros::spin();

  return 0;
}
```

# 作成したノードをビルド
作成した`sample_c_publisher`と`sample_c_subscriber`をビルド対象とするため`CMakeLists.txt`の最後に以下を追加します。

```cmake:CMakeLists.txt
include_directories(include ${catkin_INCLUDE_DIRS})

add_executable(sample_c_publisher src/sample_c_publisher.cpp)
target_link_libraries(sample_c_publisher ${catkin_LIBRARIES})

add_executable(sample_c_subscriber src/sample_c_subscriber.cpp)
target_link_libraries(sample_c_subscriber ${catkin_LIBRARIES})
```  
ビルドします。

```
$ cd ~/catkin_ws
$ catkin_make
```

# ROSを実行する
コンソールを３つ立ち上げ、それぞれ以下を実行します

```
$ roscore
```
```
$ rosrun sample_c sample_c_publisher
```
```
$ rosrun sample_c sample_c_subscriber
```

コンソールに以下の内容が出力されたら成功です。

```sample_c_publisher
[ INFO] [1550933269.893326530]: message = hello world 0, count = 0
[ INFO] [1550933270.393364511]: message = hello world 1, count = 1
[ INFO] [1550933270.893353746]: message = hello world 2, count = 2
[ INFO] [1550933271.393346524]: message = hello world 3, count = 3
...
```
```sample_c_subscriber
[ INFO] [1550933280.393826056]: I heard: message = [hello world 21], count = [21]
[ INFO] [1550933280.893708184]: I heard: message = [hello world 22], count = [22]
[ INFO] [1550933281.393709209]: I heard: message = [hello world 23], count = [23]
...
```
※sample_c_subscriberの実行タイミングによってカウンタの値がずれます

# 参考
- [公式チュートリアル](http://wiki.ros.org/ja/ROS/Tutorials)
 - [パッケージの作成](http://wiki.ros.org/ja/ROS/Tutorials/CreatingPackage)
 - [メッセージとサービスの作成](http://wiki.ros.org/ja/ROS/Tutorials/CreatingMsgAndSrv)
 - [パブリッシャとサブスクライバの作成](http://wiki.ros.org/ja/ROS/Tutorials/WritingPublisherSubscriber%28c%2B%2B%29)