前回の内容

その２

今回の内容

準備

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

ワークスペースの作成

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

作業用ディレクトリの作成(名前はなんでもよいが、~/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の通信を理解していきました。
次回もその続きをやっていくのですが、ついでにシミュレーターも使ってみようと思います。

その４