$ roscd sandbox
$ roscreate-pkg learning_actionlib actionlib roscpp rospy roslib std_msgs actionlib_msgs
$ rosmake learning_actionlib
$ roscd learning_actionlib
して開始です。
今回はアクションサーバを作ります。
http://www.ros.org/wiki/actionlib_tutorials/Tutorials/SimpleActionServer(ExecuteCallbackMethod)
その前にここを見てactionlibの概念をおさえましょう。
http://www.ros.org/wiki/actionlib
概要を見ると、actionlibとは処理に時間がかかるサービス(srv)みたいなもんだ、とあります。
ROSのserviceでは呼び出すと処理が終わるまでロックします。
これに対しactionlibではロックせずに、処理待ちなどを行います。
このサーバーとクライアント構造を簡単につくるのがactionlibになります。
じゃあ、なんで最初からそういうのができるようなフレームワークにしなかったんだ？
という気がしますが、シンプルにするとこうなるんでしょうか？
callbackを使って通常のノードと接続し、ROSのメッセージプロトコルでActionClientとActionServerは通信します。
クライアントとサーバをつなぐメッセージは以下の３つの情報を含みます。
- Goal・・・・目標値。ロボットの目標移動地点など。
- Feedback・・サーバーの目標に対する進捗状況を示す。現在のロボット位置など。
- Result・・・達成した結果を返す。ロボットの到達位置・姿勢など。
この３つを定義する.actionファイルから.msgファイルを生成し、これらを使ってActionClient, ActionServerを実装する、という流れになります。
では早速。以下をaction/Fibonacci.actionとして保存します。（mkdir actionしてください）
ファイル名でもろばれですね。
#goal definition
int32 order
---
#result definition
int32[] sequence
---
#feedback
int32[] sequence
以下のようにしてactionファイルをコンパイルします。
$ roscd learning_actionlib $ rosrun actionlib_msgs genaction.py . Fibonacci.action
もしくはCMakeLists.txtのrosbuild_initより前に以下のように書いて、
rosbuild_find_ros_package(actionlib_msgs)
include(${actionlib_msgs_PACKAGE_PATH}/cmake/actionbuild.cmake)
genaction()
rosbuild_genmsg()
も有効にしておくと自動でコンパイルしてくれます。
これで以下のように７つのメッセージが生成されました。
$ ls msg
FibonacciAction.msg FibonacciActionResult.msg FibonacciResult.msg
FibonacciActionFeedback.msg FibonacciFeedback.msg
FibonacciActionGoal.msg FibonacciGoal.msg
FibonacciAction.msgからすべてのメッセージがつながっています。
ではサーバーの実装に移りましょう。
サーバーだからサービスを提供する側ですね。
以下をsrc/fibonacci_server.cppとして保存してください。
#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include <learning_actionlib/FibonacciAction.h>
class FibonacciAction
{
protected:
ros::NodeHandle nh_;
actionlib::SimpleActionServer<learning_actionlib::FibonacciAction> as_;
std::string action_name_;
// create messages that are used to published feedback/result
learning_actionlib::FibonacciFeedback feedback_;
learning_actionlib::FibonacciResult result_;
public:
FibonacciAction(std::string name) :
as_(nh_, name, boost::bind(&FibonacciAction::executeCB, this, _1)),
action_name_(name)
{
}
~FibonacciAction(void)
{
}
void executeCB(const learning_actionlib::FibonacciGoalConstPtr &goal)
{
// helper variables
ros::Rate r(1);
bool success = true;
// push_back the seeds for the fibonacci sequence
feedback_.sequence.clear();
feedback_.sequence.push_back(0);
feedback_.sequence.push_back(1);
// publish info to the console for the user
ROS_INFO("%s: Executing, creating fibonacci sequence of order %i with seeds %i, %i", action_name_.c_str(), goal->order, feedback_.sequence[0], feedback_.sequence[1]);
// start executing the action
for(int i=1; i<=goal->order; i++)
{
// check that preempt has not been requested by the client
if (as_.isPreemptRequested())
{
ROS_INFO("%s: Preempted", action_name_.c_str());
// set the action state to preempted
as_.setPreempted();
success = false;
break;
}
feedback_.sequence.push_back(feedback_.sequence[i] + feedback_.sequence[i-1]);
// publish the feedback
as_.publishFeedback(feedback_);
// this sleep is not necessary, the sequence is computed at 1 Hz for demonstration purposes
r.sleep();
}
if(success)
{
result_.sequence = feedback_.sequence;
ROS_INFO("%s: Succeeded", action_name_.c_str());
// set the action state to succeeded
as_.setSucceeded(result_);
}
}
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "fibonacci");
FibonacciAction fibonacci(ros::this_node::getName());
ros::spin();
return 0;
}
今回は長いですね・・・。
順にみていきましょう。
#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
actionlib/server/simple_action_server.hを使って簡単に作りましょう、ということですね。
#include <learning_actionlib/FibonacciAction.h>
これはさっき作ったメッセージファイルから自動で生成されます。
メッセージの定義ファイルです。
protected:
ros::NodeHandle nh_;
actionlib::SimpleActionServer<learning_actionlib::FibonacciAction> as_;
std::string action_name_;
// create messages that are used to published feedback/result
learning_actionlib::FibonacciFeedback feedback_;
learning_actionlib::FibonacciResult result_;
これがメンバ変数。SimpleActionServerのインスタンスを持っちゃっています。
learning_actionlib::FibonacciFeedbackとFibonacciResultのインスタンスも持ちます。
FibonacciAction(std::string name) :
as_(nh_, name, boost::bind(&FibonacciAction::executeCB, this, _1)),
action_name_(name)
{
}
これがコンストラクタ。SimpleActionServerを初期化します。
引数はノードハンドラ(nh_)、サーバの名前(name）、コールバック(boost::bind())です。
コールバックの中身が実体になります。
void executeCB(const learning_actionlib::FibonacciGoalConstPtr &goal)
{
で、これがコールバックの宣言部分で、goalへのポインタを引数に取ります。
中身は以下のようにつづきます。
// helper variables
ros::Rate r(1);
bool success = true;
// push_back the seeds for the fibonacci sequence
feedback_.sequence.clear();
feedback_.sequence.push_back(0);
feedback_.sequence.push_back(1);
// publish info to the console for the user
ROS_INFO("%s: Executing, creating fibonacci sequence of order %i with seeds %i, %i", action_name_.c_str(), goal->order, feedback_.sequence[0], feedback_.sequence[1]);
１Hzで、まずfeedbackに、0と1をつめてます。
// start executing the action
for(int i=1; i<=goal->order; i++)
{
// check that preempt has not been requested by the client
if (as_.isPreemptRequested())
{
ROS_INFO("%s: Preempted", action_name_.c_str());
// set the action state to preempted
as_.setPreempted();
success = false;
break;
}
クライアントが途中でActionを止めたくなったときに、止められるようにisPreemptRequested()で停止したときの処理を行います。setPreempted()を呼び出す必要があります。
feedback_.sequence.push_back(feedback_.sequence[i] + feedback_.sequence[i-1]);
// publish the feedback
as_.publishFeedback(feedback_);
// this sleep is not necessary, the sequence is computed at 1 Hz for demonstration purposes
r.sleep();
}
ここがメインの処理（といっても二つの数字を足しているだけですが・・・）
途中で停止させる演出のためにわざと遅くして１Hzになっています。
publishFeedback()によりfeedbackチャネルに書き込まれます。
if(success)
{
result_.sequence = feedback_.sequence;
ROS_INFO("%s: Succeeded", action_name_.c_str());
// set the action state to succeeded
as_.setSucceeded(result_);
}
}
ユーザにより停止されなければここが実行されて、
setSucceeded()で結果がresultというチャネルで書き込まれます。
int main(int argc, char** argv)
{
ros::init(argc, argv, "fibonacci");
FibonacciAction fibonacci(ros::this_node::getName());
ros::spin();
return 0;
}
メインプログラムはFibonacciActionのインスタンスを作っておしまいです。
ros::spin()でコールバックを待ちます。
ではビルドしましょう。
rosbuild_add_executable(fibonacci_server src/fibonacci_server.cpp)
を加えて、さらに
rosbuild_genmsg()をコメントインします。
これでメッセージがコンパイルされます。
そしてmakeしてください。
$ roscore
$ rosrun learning_actionlib fibonacci_server
まだクライアントがないので、なにも起きません。
$ rostopic list -v
として、トピックが用意されていることだけ確認しましょう。
$ rxgraph -t
とすると、トピックも可視化できます。
すごい。。。
では次回はこれを使うクライアントを作りましょう。
0 件のコメント:
コメントを投稿