この記事は私が担当している講義ロボットプログラミングⅡ用です。今回はROSのサービスを理解しましょう。次のROS Wikiを参考にしています。
サービスはROSの通信方法の一つで、双方向通信に使います。ある仕事をお願いするクライアントとそれを処理して返すサーバーからなります。今回、作成するプログラムは速度指令値を送るクライアントと、それをロボットへ伝え(リクエスト）、ロボットから現在の速度を取得しクライアントに返す（レスポンス）サーバーです。次の手順でやりましょう。この例は、前回学んだトピック通信も入れていますので、実践的な内容です。
１．my_serviceパッケージの作成
$ cd ~/catkin_ws/src
$ catkin_create_pkg my_service std_msgs rospy roscpp
２．srvファイルの作成。srvファイルはやり取りするデータの型を表すテキストファイル。拡張子がsrv、この例のファイル名はWheel.srv。
$ cd ~/catkin_ws/src/my_service
$ mkdir srv
$ cd srv
$ gedit Wheel.srv
内容は次のコード。——-の上が入力（リクエスト）、下が出力（レスポンス）を表す。
float64 target_linear_velocity float64 target_angular_velocity --------------- float64 current_linear_velocity float64 current_angular_velocity
並進、回転速度指令値target_linear_velocity, target_angular_velocityがリクエストされると、
現在の並進、回転速度current_linear_velocity, current_angular_velocityがリスポンスされる。
３．Package.xmlの変更
~/catkin_ws/src/my_service/Package.xmlの35、39行目のコメントタグを次のように外す。message_generation, message_runtimeを有効にする必要あり。
<!-- Use build_depend for packages you need at compile time: --> <build_depend>message_generation</build_depend> <!-- Use buildtool_depend for build tool packages: --> <!-- <buildtool_depend>catkin</buildtool_depend> --> <!-- Use run_depend for packages you need at runtime: --> <run_depend>message_runtime</run_depend>
４．CMakeLists.txtの変更
~/catkin_ws/src/my_service/CMakeLists.txtの11行目、std_msgsの下にmessage_generationを追加。
find_package(catkin REQUIRED COMPONENTS roscpp rospyp std_msgs message_generation )
次の54~59行目を
## Generate services in the 'srv' folder # add_service_files( # FILES # Service1.srv # Service2.srv # )
以下に変更。
## Generate services in the 'srv' folder add_service_files( FILES Wheel.srv )
次の70~74行目
## Generate added messages and services with any dependencies listed here # generate_messages( # DEPENDENCIES # std_msgs # )
のコメントを外し、次にする。
## Generate added messages and services with any dependencies listed here
generate_messages(
DEPENDENCIES
std_msgs
)
次に示す102~107行目
#catkin_package( # INCLUDE_DIRS include # LIBRARIES my_service # CATKIN_DEPENDS roscpp rospy std_msgs # DEPENDS system_lib #)
を次に変更
catkin_package( INCLUDE_DIRS include LIBRARIES my_service CATKIN_DEPENDS roscpp rospy std_msgs message_runtime DEPENDS system_lib )
５．ビルド
ソースコードを書く前にサービスのライブラリを作成するためにcatkin_makeしてビルドする。
$ cd ~/catkin_ws
$ catkin_make
ビルド時にエラーがでたら、Package.xml, CMakelists.txtに間違えがないか確認し、直して再度ビルド。
６．サービスサーバーのソースコード作成
では、依頼された仕事を処理するサーバープログラムを作ります。次のプログラムをgeditなどを使いwheel_server.cppというファイル名を付けて~/catkin_ws/src/my_service/src/wheel_server.cppと保存。一字でも打ち間違えると動かないのでコピペする。
#include "ros/ros.h"
#include <geometry_msgs/Twist.h>
#include "nav_msgs/Odometry.h"
#include "my_service/Wheel.h" // 自動的に作られる
class Server {
public:
Server(); // コンストラクタ
~Server() {}; // デストラクタ
// オドメトリのコールバック関数
void odomCallback(const nav_msgs::Odometry::ConstPtr& odom);
// サーバーのコールバック関数（サービスの本体）
bool wheelService(my_service::Wheel::Request &req,
my_service::Wheel::Response &res);
void loop(); // ループ関数
private:
ros::Publisher cmd_pub; // パブリッシャ
ros::Subscriber odom_sub; // サブスクライバ
ros::NodeHandle nh; // ノードハンドルの宣言
ros::ServiceServer service; // サービス
double tmp_linear_velocity; // 現在の並進速度
double tmp_angluar_velocity; // 現在の回転速度
const static double max_linear_velocity = 0.7; // 最大並進速度
const static double min_linear_velocity = -0.7; // 最小並進速度
const static double max_angular_velocity = 1.2; // 最大回転速度
const static double min_angular_velocity = -1.2; // 最小回転速度
geometry_msgs::Twist target_vel, tmp_vel; // 目標速度、現在の速度
};
// コンストラクタ
Server::Server()
{
ROS_INFO("Ready to wheel");
//サービスの設定
service = nh.advertiseService("wheel",&Server::wheelService,this);
// パブリッシャ(配信)の設定:
cmd_pub= nh.advertise<geometry_msgs::Twist>("/mobile_base/commands/velocity", 10);
// サブスクライバ(購読)の設定
// /odomトピックはロボットの速度情報を持っている
odom_sub = nh.subscribe("/odom", 10, &Server::odomCallback, this);
// 速度の初期化
target_vel.linear.x = 0.0;
target_vel.linear.y = 0.0;
target_vel.linear.z = 0.0;
target_vel.angular.x = 0.0;
target_vel.angular.y = 0.0;
target_vel.angular.z = 0.0;
tmp_vel.linear.x = 0.0;
tmp_vel.linear.y = 0.0;
tmp_vel.linear.z = 0.0;
tmp_vel.angular.x = 0.0;
tmp_vel.angular.y = 0.0;
tmp_vel.angular.z = 0.0;
}
// サーバーのコールバック関数
bool Server::wheelService(my_service::Wheel::Request &req,
my_service::Wheel::Response &res)
{
ROS_INFO("Set velocity: linear=%.2f angular=%.2f",
req.target_linear_velocity, req.target_angular_velocity);
// 指令速度が最小、最大速度内かチェック
if (min_linear_velocity > req.target_linear_velocity ||
max_linear_velocity < req.target_linear_velocity) return false;
if (min_angular_velocity > req.target_angular_velocity ||
max_angular_velocity < req.target_angular_velocity) return false;
// クライアントからの速度をロボットの速度指令とする
target_vel.linear.x = req.target_linear_velocity;
target_vel.angular.z = req.target_angular_velocity;
// /odomトピックからサブスクライブした速度情報をクライアントに返す
res.current_linear_velocity = tmp_vel.linear.x;
res.current_angular_velocity = tmp_vel.angular.z;
return true;
}
// オドメトリのコールバック関数（現在速度を知る）
void Server::odomCallback(const nav_msgs::Odometry::ConstPtr& odom)
{
tmp_vel = odom->twist.twist; // 現在の速度ゲット
}
// ループ関数
void Server::loop()
{
ros::Rate loop_rate(30); // Hz
while(ros::ok()) {
// ロボットを動かすため目標速度をパブリッシュ
cmd_pub.publish(target_vel);
// コールバック関数を呼ぶ
ros::spinOnce();
// 決められた周期でループするため寝て待つ
loop_rate.sleep();
}
}
int main(int argc, char **argv)
{
//ROSの初期化 wheel_serverはノード名
ros::init(argc, argv, "wheel_server");
Server svr;
svr.loop();
return 0;
}
７．サービスクライアントのソースコード作成
次に仕事を依頼するのクライアントのソースコートを作成する。以下のソースコードをwheel_client.cppという名前で~/catkin_ws/src/my_service/src/wheel_client.cpp保存する。
#include "ros/ros.h"
#include "my_service/Wheel.h" // 自動的に作られる
int main(int argc, char **argv)
{
// 初期化 wheel_clientノード
ros::init(argc, argv, "wheel_client");
ros::NodeHandle nh;
// サービスクライアントの設定
ros::ServiceClient client = nh.serviceClient<my_service::Wheel>("wheel");
my_service::Wheel srv;
std::cout << "Input target linear velocity:";
std::cin >> srv.request.target_linear_velocity;
std::cout << "Input target angular velocity:";
std::cin >> srv.request.target_angular_velocity;
// サービスを呼ぶ
if (client.call(srv)) {
// 成功したらサーバーからのレスポンスを表示
ROS_INFO("Current linear_vel=%f angular_vel=%f",
(double) srv.response.current_linear_velocity,
(double) srv.response.current_angular_velocity);
}
else {
// 失敗したらエラー表示
ROS_ERROR("Faild to call service wheel");
return 1;
}
return 0;
}
８．CMakeLists.txtの変更とビルド
以下をCMakeLists.txtに追加
## Declare a C++ executable
add_executable(wheel_server src/wheel_server.cpp)
add_dependencies(wheel_server my_service_gencpp)
target_link_libraries(wheel_server ${catkin_LIBRARIES})
add_executable(wheel_client src/wheel_client.cpp)
add_dependencies(wheel_client my_service_gencpp)
target_link_libraries(wheel_client ${catkin_LIBRARIES})
$ cd ~/catkin_ws
$ catkin_make
９．実行
端末を４つ開き、各端末で以下のコマンドを実行
$ roslaunch turtlebot_gazebo turtlebot_world.launch
$ roslaunch turtlebot_bringup minimal.launch
$ rosrun my_service wheel_server
サーバーは次のように表示される。
$ rosrun my_service wheel_client
クライアントはキーボードから入力（速度指令値）をサーバーへ送る。速度指令値が最小、最大値の範囲外ならエラーを返す。それ以外は、シミュレータ上のロボットが動きます。
成功したら終わり。うまく動かない場合は、打ち間違えや手順に間違いがないか確認し、再度実行しましょう。
なお、今回のソースコードをまとめてここmy_service.tgzに置いておきます。
解凍の仕方は次の通り。
$ cp ./my_service.tgz ~/catkin_ws/src
$ cd ~/catkin_ws/src
$ tar xvzf my_service.tgz
コメント