| Note: This tutorial assumes that you have completed the previous tutorials: Running a realtime joint controller. |
 Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
Communicating with a realtime joint controller
Description: このチュートリアルはあなたにROSの上にコントローラーとコミュニケートする方法を教えます。Tutorial Level: INTERMEDIATE
Next Tutorial: Adding a PID to a realtime joint controller
Introduction
このチュートリアルは我々がprevious tutorialに作ったコントローラーをもとに作り上げます。ここで我々はその場その場でアームの運動の振幅を変えるために、コントローラーにROS serviceコールを加えるでしょう。もしあなたがまだROS serviceがなんであるか知らないなら、最初に ROS tutorialsをチェックしてください。
Adding the service
最初に我々は我々がコントローラーとコミュニケートするために使うであろうサービスコールを指定します。我々がすることを望む唯一のことは振幅を設定することですから、サービスのリクエスト("---"上)はただ1つの値を持っているだけです。サービス応答（"---"下）は設定された実際の振幅を含んでいます。もしリクエストされた振幅が無効であるなら、リクエストと応答は異なっているかもしれません。それで、最初にディレクトリsrv、srv/SetAmplitude.srvと呼ばれるファイル、を作って、そして次のサービス記述をコピー-ペーストしてください
float64 amplitude --- float64 amplitude
それから、我々はcmakeに上のサービス記述からC++ヘッダーを作成するように告げる必要があります。従って簡単にあなたのCMakeLists.txtに次のラインを加えてください：
rosbuild_gensrv()
Changing the code
今我々は我々のコントローラーコードにサービスコールを加える必要があります。（下に見る）結果として生じているコードはオリジナルのコントローラーコードに非常に類似しています。
include/my_controller_pkg/my_controller_file.h
1 #include <pr2_controller_interface/controller.h>
2 #include <pr2_mechanism_model/joint.h>
3 #include <ros/ros.h>
4 #include <my_controller_pkg/SetAmplitude.h>
5
6 namespace my_controller_ns{
7
8 class MyControllerClass: public pr2_controller_interface::Controller
9 {
10 private:
11 bool setAmplitude(my_controller_pkg::SetAmplitude::Request& req,
12 my_controller_pkg::SetAmplitude::Response& resp);
13
14 pr2_mechanism_model::JointState* joint_state_;
15 double init_pos_;
16 double amplitude_;
17 ros::ServiceServer srv_;
18
19 public:
20 virtual bool init(pr2_mechanism_model::RobotState *robot,
21 ros::NodeHandle &n);
22 virtual void starting();
23 virtual void update();
24 virtual void stopping();
25 };
26 }
ヘッダーファイルの変更:
ここで我々は我々がサービスを定義するためにrosヘッダーファイルと同様、ただ作成した新しいサービスコールを定義する自動的に生み出されたヘッダーファイルを含みます。
ここで我々は、サービスが呼び出されるとき、呼ばれるであろうメソッドを宣言します。
ここで我々はサービスプロバイダとローカルな変数がアームの運動の振幅をストアすると宣言します。
src/my_controller_file.cpp
1 #include "my_controller_pkg/my_controller_file.h"
2 #include <pluginlib/class_list_macros.h>
3
4 namespace my_controller_ns {
5
6
7 /// Controller initialization in non-realtime
8 bool MyControllerClass::init(pr2_mechanism_model::RobotState *robot,
9 ros::NodeHandle &n)
10 {
11 std::string joint_name;
12 if (!n.getParam("joint_name", joint_name))
13 {
14 ROS_ERROR("No joint given in namespace: '%s')",
15 n.getNamespace().c_str());
16 return false;
17 }
18
19 joint_state_ = robot->getJointState(joint_name);
20 if (!joint_state_)
21 {
22 ROS_ERROR("MyController could not find joint named '%s'",
23 joint_name.c_str());
24 return false;
25 }
26
27 srv_ = n.advertiseService("set_amplitude",
28 &MyControllerClass::setAmplitude, this);
29
30 amplitude_ = 0.5;
31 return true;
32 }
33
34
35 /// Controller startup in realtime
36 void MyControllerClass::starting()
37 {
38 init_pos_ = joint_state_->position_;
39 }
40
41
42 /// Controller update loop in realtime
43 void MyControllerClass::update()
44 {
45 double desired_pos = init_pos_ + amplitude_ * sin(ros::Time::now().toSec());
46 double current_pos = joint_state_->position_;
47 joint_state_->commanded_effort_ = -10 * (current_pos - desired_pos);
48 }
49
50
51 /// Controller stopping in realtime
52 void MyControllerClass::stopping()
53 {}
54
55
56 /// Service call to set amplitude of sin
57 bool MyControllerClass::setAmplitude(my_controller_pkg::SetAmplitude::Request& req,
58 my_controller_pkg::SetAmplitude::Response& resp)
59 {
60 if (fabs(req.amplitude) < 2.0){
61 amplitude_ = req.amplitude;
62 ROS_INFO("Mycontroller: set amplitude too %f", req.amplitude);
63 }
64 else
65 ROS_WARN("MyController: requested amplitude %f too large", req.amplitude);
66
67 resp.amplitude = amplitude_;
68 return true;
69 }
70 } // namespace
71
72 /// Register controller to pluginlib
73 PLUGINLIB_REGISTER_CLASS(MyControllerPlugin,
74 my_controller_ns::MyControllerClass,
75 pr2_controller_interface::Controller)
cppファイルの変更は:
ここで我々はサービスコールを作って、それに名前を与えて、そしてそれをsetAmplitudeメソッドと接続します。
45 double desired_pos = init_pos_ + amplitude_ * sin(ros::Time::now().toSec());
アップデートループで我々はアームの運動の振幅を指定する新しいローカルな変数を使います。
56 /// Service call to set amplitude of sin
57 bool MyControllerClass::setAmplitude(my_controller_pkg::SetAmplitude::Request& req,
58 my_controller_pkg::SetAmplitude::Response& resp)
59 {
60 if (fabs(req.amplitude) < 2.0){
61 amplitude_ = req.amplitude;
62 ROS_INFO("Mycontroller: set amplitude too %f", req.amplitude);
63 }
64 else
65 ROS_WARN("MyController: requested amplitude %f too large", req.amplitude);
66
67 resp.amplitude = amplitude_;
68 return true;
69 }
ここで我々はそれ自身サービスコールを実行します。 もしそれが2.0以下であるなら、リクエストされた振幅がどのようにただ受け入れられるだけであるかに注意を払ってください。
Testing the communication
それで今我々はコントローラーへのこの拡張をテストする準備ができています。 最初にコードをライブラリにコンパイルし、コントローラーをビルドしてください:
$ make
この新しいコードがリアルタイムプロセスにロードされるために、あなたはgazeboとこれから全過程を再開する必要があるか、あるいは pr2_controller_managerをライブラリのリロードを求めるために使います。
それで、もしあなたがすでにgazebo走らせていないなら、タイプしてください：
$ roslaunch gazebo_worlds empty_world.launch $ roslaunch pr2_gazebo pr2.launch
けれどももしgazeboが前のチュートリアルからまだ走っていたなら、走らせてください：
$ rosrun pr2_controller_manager pr2_controller_manager reload-libraries
そして今、我々の新しいコントローラーをスタートさせてください：
$ roslaunch my_controller.launch
今、このまったく新しいサービスを呼び出しましょう：
$ rosservice call /my_controller_name/set_amplitude 0.1
あなたは応答が0.1であるのを見るべきです、そしてアームはより小さい振幅で動いてスタートするべきです。
もしあなたが2.0より大きい値に振幅を設定しようとするなら
$ rosservice call /my_controller_name/set_amplitude 2.9
あなたは応答が振幅をあなたが設定した最後のときから変わらないのを見るでしょう。 大きい振幅は合法的に拒絶されます。
あなたは今次のチュートリアルをスタートして、方法add a PID to your controllerを学ぶ準備ができています。
