ROSを使用したCRANE+の動かし方 その7

こんにちは!長谷川です。
前回はCRANE+の全ての関節を同時に、かつ自動で動かす方法について説明しましたが、今回はその時出てきたソースコードの解説を通して、ROSのプログラムの書き方について説明したいと思います。

プログラムについてのチュートリアル

ここにC++でのプログラムの書き方が載っていまして、ここ
に実行の仕方が載っています。以上二つに基づいて解説を進めていきます。

ROSで使えるプログラミング言語

ROSでは、PythonとC++の二種類の言語を使うことができます。これは便利なことに思えますが、実はいろいろとめんどくさいです。例えば、C++で書かれたノードがトピックを介してPythonで書かれたノードにデータを送るとしましょう。もしトピックのデータ型を、ノードを起動するプログラム内でfloatとして定義できるとしたら、そのデータ型をPythonにそのまま引き継げるでしょうか?言語が違えばデータ型の定義も違うので、難しいことになります。そこで、トピックのデータ型などは言語によらず共通に定義されています。今の例でいえば、トピックのデータ型をstd_msgs::Float64と定義します。std_msgs::Float64はROS内でPythonでも使えるデータ型になっているので、データを正確に受け渡しできるというわけです。

プログラムの解読

それでは、前回のプログラムを解読していきましょう。
[cpp firstline=”1″]#include "ros/ros.h"[/cpp]
お馴染みのヘッダーのインクルードですが、ros/ros.hをインクルードすると、ROS のシステムでよく用いる部分を使うのに必要なヘッダーをすべてインクルードできます。
[cpp firstline=”6″]#include "std_msgs/Float64.h"[/cpp]
このヘッダーのインクルードにより、先ほど出てきたデータ型、std_msgs::Float64が使えるようになります。今回、CRANE+を動かすために用意したトピックのデータ型はすべてこれなので、インクルードが必要です。
[cpp firstline=”8″]#include <sstream>[/cpp]
文字列ストリームを使うためのヘッダーです。今回は使わないのでいらないのですが、チュートリアル見ながらプログラムを作っていたときに入れてしまいました。
[cpp firstline=”13″]ros::init(argc, argv, "turtlebot_arm");[/cpp]
ROSの初期化をして、このプログラムで起動するノードの名前を指定します。今回は「turtlebot_arm」です。名前には、/を含んではいけないみたいです。
[cpp firstline=”15″]ros::NodeHandle n;[/cpp]
ノードへのハンドラなるものを作成します。ここではnという名前で作成しました。ハンドラとは名前空間を管理するものです。ちなみに、ここでノードの初期化が行われ、最後のハンドラが破棄されるとノードの終了処理に入ります。
[cpp firstline=”18″]
ros::Publisher joint1_pub = n.advertise<std_msgs::Float64>("/tilt1_controller/command", 1000);
[/cpp]
ここで、std_msgs::Float64型のトピック/tilt1_controller/commandに、データの配信を行うことを宣言します。テンプレートを使った宣言です。1000というのは、蓄えることのできるデータの最大数です。トピックにデータが配信されると、そのトピックが購読されるまでデータが保持されなければなりませんが、トピックが購読される前に次の配信が行われてしまう可能性があります。この時、前のデータがすぐ消えないように、キューで値を保持します。このキューの個数が1000個ということです。joint1_pubというros::Publisher型インスタンス(メモリを確保したクラス)が定義されていますが、これは後ですぐに使います。
[cpp firstline=”23″]std_msgs::Float64 pos1;[/cpp]
std_msgs::Float64型のインスタンスpos1を宣言します。これを経由してデータの配信を行います。
[cpp firstline=”29″]pos1.data = 0.0;[/cpp]
pos1のメンバ変数dataに、0.0という値を代入します。
[cpp firstline=”34″]joint1_pub.publish(pos1);[/cpp]
18行目で宣言したjoint1_pubのpublishメソッドを用いることで、トピックへのデータの配信が行えます。この場合は、引数をpos1に設定しているので、pos1がトピックへ配信されます。実質的には、トピック/tilt1_controller/commandに、0.0という値を配信しているようなものです。この値がID1のサーボモータの回転角になることは前回述べた通りなので、サーボモータに回転角を指示していることになります。
[cpp firstline=”40″]ros::Rate loop_rate(0.5);[/cpp]
これで、ループ頻度を設定することができます。この場合は、0.5Hzで駆動するということです。具体的には、loop_rate.sleep()を呼び出してからどれだけ時間が経過したかを常に管理し、次にloop_rate.sleep()が呼び出された際、前回のスリープが終わってから2秒たつまでスリープします。
[cpp firstline=”42″]while (ros::ok())[/cpp]
ros::ok()というのは、以下のような場合にfalseを返す関数です。
・Ctrl-Cが押され、SIGINTシグナルを受け取った時。
・同じ名前のノードができたために,ネットワークから外された時。
・アプリケーションの他の部分でros::shutdown()が呼ばれた時。
・全ての ros::NodeHandles が破棄された時。
以上の場合、falseが返されてwhile文から抜けるため、プログラムが終了します。
[cpp firstline=”184″]ros::spinOnce();[/cpp]
トピックを購読する場合は必須の処理です。これがないとコールバックが呼ばれず、トピックを購読することができません。トピックを購読しようと思っている場所では、必ず繰り返し呼び出すようにしてください。

以上で、ROSのプログラムの書き方の基本は説明できたと思います。次回は、CRANE+のURDFモデルの作り方について説明したいと思います。