ROSを始めよう　その３

# 前回の内容
[その２](https://qiita.com/hagi-suke/items/fde0dab42294f569a0fe)

# 今回の内容
前回のようなコマンド操作ではなく、プログラムを書くことでROSを理解していきましょう。
参考：http://wiki.ros.org/ja/ROS/Tutorials/InstallingandConfiguringROSEnvironment
　　　http://wiki.ros.org/ja/ROS/Tutorials/NavigatingTheFilesystem
　　　http://wiki.ros.org/ja/ROS/Tutorials/CreatingPackage
　　　http://wiki.ros.org/ja/ROS/Tutorials/BuildingPackages
　　　http://wiki.ros.org/ja/ROS/Tutorials/UsingRqtconsoleRoslaunch
　　　http://wiki.ros.org/ja/ROS/Tutorials/WritingPublisherSubscriber%28c%2B%2B%29
　　　http://wiki.ros.org/ja/ROS/Tutorials/WritingPublisherSubscriber%28python%29
　　　http://wiki.ros.org/ja/ROS/Tutorials/ExaminingPublisherSubscriber
　　　http://wiki.ros.org/ja/ROS/Tutorials/WritingServiceClient%28c%2B%2B%29
　　　http://wiki.ros.org/ja/ROS/Tutorials/WritingServiceClient%28python%29
　　　http://wiki.ros.org/ja/ROS/Tutorials/ExaminingServiceClient

# 準備
実際にプログラムを書く前に、いろいろとやらなければいけないことがあります。

## ワークスペースの作成
プログラムなどを入れておく場所として、作業用のディレクトリが必要です。これをワークスペースと呼びます。

```
作業用ディレクトリの作成(名前はなんでもよいが、~/catkin_wsが一般的)
$ mkdir -p ~/catkin_ws/src
ディレクトリ移動
$ cd ~/catkin_ws/src
catkin用のワークスペースを作るコマンド(この1回だけ行えばよい)
$ catkin_init_workspace
ワークスペースに移動
$ cd ~/catkin_ws
一度ビルドする(このコマンドはROSのプログラムをビルドするときに使います)
$ catkin_make
```
最後のコマンドで表示されるのが、青い文字なら大丈夫です。
ここででてきたcatkinというのは、ROSのビルドシステムで、ROSのプログラムをビルドするときなどに使います。
catkin_makeコマンドによってdevelやbuildといったディレクトリも自動で生成されているはずですので、以下のようにsetup.bashを読み込みましょう。

```
$ source ~/catkin_ws/devel/setup.bash
```
このコマンドは毎回必要になるので、~/.bashrcの最下行のsource /opt/ros/kinetic/setup.bashを消して、そこに書き込んでしまいましょう。

ワークスペースがちゃんと作れたかどうかは`$  echo $ROS_PACKAGE_PATH`で確認できます。

## パッケージの作成
ROSではすべてのプログラムはいずれかのパッケージというものに所属することになっています。パッケージとは、固まった機能をもったプログラムの集まりのことです。

```
ディレクトリ移動
$ cd ~/catkin_ws/src
ros_beginnerという名前のパッケージ作成(後に続くのは利用したい外部パッケージ名)
$ catkin_create_pkg ros_beginner rospy roscpp std_msgs
ワークスペースに移動
$ cd ~/catkin_ws
一度ビルドする
$ catkin_make
setup.bashの読み込み
$ source ~/catkin_ws/devel/setup.bash
```
catkin_create_pkgコマンドで利用したい外部パッケージが現段階で分からなければ、後で追加することもできるので安心してください。今回はとりあえず基本的なrospy、roscpp、std_msgsを並べました。
成功していれば、ワークスペース~/catkin_wsで`$ roscd ros_beginneer`と打つと~/catkin_ws/src/ros_startに移動できます。

# Topic通信をしてみよう(Python)
準備もできたので、プログラムを書いていきましょう。
## ディレクトリを用意

```
パッケージのディレクトリに移動
$ roscd ros_beginner
Pythonのスクリプトを入れるディレクトリを作成
$ mkdir scripts
移動
$ cd scripts
```

## Publisher作成

```talker.py
#! /usr/bin/env python
import rospy
from std_msgs.msg import String

def talker():
    # talkerという名前のNodeにする
    rospy.init_node('talker', anonymous=True)
    # chatterという名前のString型のTopicにPublishするPublisherを作成
    pub = rospy.Publisher('chatter', String, queue_size=10)
    # 10Hzで定期的にプログラム実行する仕組み
    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        # 送るMassage(データ)を用意
        hello_str = String()
        hello_str.data = "hello world %s" % rospy.get_time()
        # 実際にPublishする
        pub.publish(hello_str)
        # 10Hzに合わせて必要な時間sleepする
        rate.sleep()

if __name__ == '__main__':
    try:
        talker()
    except rospy.ROSInterruptException: pass
```

## Subscriber作成

```listener.py
#! /usr/bin/env python
import rospy
from std_msgs.msg import String

def callback(message):
    # 文字列を表示&TopicとしてPublishする
    rospy.loginfo("I heard %s", message.data)

def listener():
    # listenerという名前のNodeにする
    rospy.init_node('listener', anonymous=True)
    # chatterという名前のString型のTopicをSubscribeし、Massageに対してcallbackを実行するSubscriberを作成
    sub = rospy.Subscriber('chatter', String, callback)
    # 無限ループをしながらMessageの受信を待つ
    rospy.spin()

if __name__ == '__main__':
    listener()
```

## プログラムを実行する
Pythonで書かれたScriptは`$ chmod 755 talker.py listener.py`とすることで、そのまま実行可能な実行ファイルとなります。(なので~/catkin_wsで`$ catkin_make`をする必要はないはずですが、うまくいかなければやってください。)
ターミナルを3つ立ち上げて、`$ roscore`、`$ rosrun ros_beginner talker.py`、`$ rosrun ros_beginner listener.py`と実行していき、listener.py側に文字列が表示されるか確認してみましょう。
このように、`$ rosrun [パッケージ名] [プログラム名(Node名)]`でプログラムを実行できます。
さらに、rospy.loginfoは文字列をTopicとしてPublishもしているので確認してみましょう。新しいターミナルで`$ rqt_console`と打つと、ログの出力が見えるはずです。

## roslaunchでまとめる
今までターミナルをたくさん用意してきましたが、複数のプログラムの実行をまとめられるツールがあります。

### ディレクトリを用意

```
パッケージのディレクトリに移動
$ roscd ros_beginner
launchファイルを入れるディレクトリを作成
$ mkdir launch
移動
$ cd launch
```

### launchファイル作成

```chat.launch
<launch>
  <node pkg="ros_beginner" name="talker" type="talker.py"/>
  <node pkg="ros_beginner" name="listener" type="listener.py" output="screen"/>
</launch>
```
このように、実行するNodeについて、パッケージ名、Nodeの名前、実行ファイル名(、必要なら出力先なども)を指定します。listenerの方は文字列を出力するので、output="screen"で画面に表示されるようにしました。

### 実行
`$roslaunch ros_beginner chat.launch`のように、`$ roslaunch [パッケージ名] [ファイル名]`で実行できます。ちなみにroslaunchで実行すると`roscore`も自動で立ち上げてくれるので今回ターミナルは1つで済みます。便利ですね。

# Topic通信をしてみよう(C++)
プログラムはros_beginner/srcの中に書いていきます。

```talker.cpp
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <sstream>

int main(int argc, char **argv)
{
  ros::init(argc, argv, "talker");
  ros::NodeHandle n;
  ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
  ros::Rate loop_rate(10);
  int count = 0;
  while (ros::ok())
  {
    std_msgs::String msg;
    std::stringstream ss;
    ss << "hello world " << count;
    msg.data = ss.str();

    ROS_INFO("%s", msg.data.c_str());
    chatter_pub.publish(msg);
    // 今回は実際には不要だが、コールバックを受ける必要があるノードの場合はこれを呼ぶ
    ros::spinOnce();

    loop_rate.sleep();
    ++count;
  }
  return 0;
}
```
次にCMakeLists.txtの108行目あたりのadd_executableを`add_executable(talker src/talker.cpp)`に変更します。(talkerはNodeの名前)
115行目あたりのtarget_link_librariesをコメントアウトし、Nodeの名前部分をtalkerに変更します。add_dependencies(talker ros_beginner_generate_messages_cpp)やinclude_directories(include ${catkin_INCLUDE_DIRS})も追加しましょう。

最後に~/catkin_wsで`$ catkin_make`すれば、実行ファイルが作成されます。
`$ ls level/lib/ros_beginner/`で確認しましょう。

`$ rosrun ros_beginner talker`で実行できます。

listener側も同様に行います。

```listener.cpp
#include <ros/ros.h>
#include <std_msgs/String.h>

void chatterCallback(const std_msgs::String::ConstPtr& msg)
{
  ROS_INFO("I heard: [%s]", msg->data.c_str());
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "listener");
  ros::NodeHandle n;
  ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);
  ros::spin();

  return 0;
}
```

# Service通信をしてみよう(Python)
## Server作成
scriptsディレクトリ内にPythonプログラムを書いていきます。

```service_server.py
#! /usr/bin/env python

import rospy
from std_srvs.srv import Empty
from std_srvs.srv import EmptyResponse

def handle_service(req):
    rospy.loginfo('called!')
    # 返り値を期待される型で返す
    return EmptyResponse()

def service_server():
    # service_serverという名前のNodeにする
    rospy.init_node('service_server')
    # call_meという名前のEmpty型で、呼ばれたらhandle_serviceを実行するServiceを作成
    s = rospy.Service('call_me', Empty, handle_service)
    print "Ready to serve."
    # 無限ループをしながらServiceが呼ばれるのを待つ
    rospy.spin()

if __name__ == '__main__':
    service_server()
```

## Client作成

```service_client.py
#! /usr/bin/env python

import rospy
from std_srvs.srv import Empty

def call_service():
    rospy.loginfo('waiting service')
    # call_meという名前のServerが立ち上がるのを待つ
    rospy.wait_for_service('call_me')
    try:
        # call_meという名前でEmpty型のClientを作成
        service = rospy.ServiceProxy('call_me', Empty)
        # Clientを呼び出す
        response = service()
    except rospy.ServiceException, e:
        print "Service call failed: %s" % e

if __name__ == "__main__":
    call_service()
```

## 実行
chmod 755で実行可能にした後、それぞれのプログラムを実行します。
service_server.pyを実行するとReady to serve.と表示されるので、service_client.pyを実行すると、Server側でcalled!と表示されます。呼び出しが成功したようです。

# Service通信をしてみよう(C++)

```service_server.cpp
#include <ros/ros.h>
#include <std_srvs/Empty.h>

bool handle_service(std_srvs::Empty::Request &req,
std_srvs::Empty::Response &res)
{
    ROS_INFO("called!");
    // 返り値を期待される型で返す
    //return std_srvs::EmptyResponse()
    return true;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "service_server");
  ros::NodeHandle n;

  ros::ServiceServer service = n.advertiseService("call_me", handle_service);
  ROS_INFO("Ready to serve.");
  ros::spin();

  return 0;
}
```

```service_client.cpp
#include <ros/ros.h>
#include <std_srvs/Empty.h>

int main(int argc, char **argv)
{
  ROS_INFO("waiting service");
  ros::init(argc, argv, "service_client");
  ros::NodeHandle n;
  ros::ServiceClient client = n.serviceClient<std_srvs::Empty>("call_me");

  std_srvs::Empty::Request req;
  std_srvs::Empty::Response res;
  if (client.call(req, res))
  {
    ROS_INFO("Recive response");
  }else{
    ROS_ERROR("Service call failed");
    return 1;
  }
  return 0;
}
```

# まとめ
今回は実際にプログラムを書くことでROSの通信を理解していきました。
次回もその続きをやっていくのですが、ついでにシミュレーターも使ってみようと思います。

[その４](https://qiita.com/hagi-suke/items/f5a4b33979d1c13bdaa4)