Show EOL distros:
Note: This tutorial assumes that you have completed the previous tutorials: 初级教程. |
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. |
使用目标回调方法(Goal Callback Method)编写一个简单的行为服务器
Description: 本教程涵盖了使用simple_action_server库来创建一个平均行为服务器。该示例展示了如何使用行为来处理或反应从ros节点中传来的数据。行为服务器平均来自ros节点的数据,目标是将多组示例去平均,反馈是简单数字、简单数据、当前平均和当前标准差,以及平均后的结果和简单数字请求的标准差。Tutorial Level: INTERMEDIATE
Next Tutorial: 编写一个简单的客户端线程
Contents
创建一个行为消息
开始编写一个行为之前,很重要的事是定义目标、结果和反馈消息。行为消息会自动从.action文件中生成,对于更多细节关于行为文件查看actionlib文档。这个文件定义了目标、结果和行为反馈话题的类型和格式。用你最喜欢的编辑器创建actionlib_tutorials/action/Averaging.action,然后用以下内容加载:
#goal definition int32 samples --- #result definition float32 mean float32 std_dev --- #feedback int32 sample float32 data float32 mean float32 std_dev
为了从这个文件中手动生成消息:
$ roscd actionlib_tutorials $ rosrun actionlib_msgs genaction.py -o msg/ action/Averaging.action
你将会看到:
Generating for action Averaging
在编译过程中,自动生成消息文件,添加以下内容到CMakeLists.txt:
find_package(catkin REQUIRED COMPONENTS actionlib std_msgs message_generation) add_action_files(DIRECTORY action FILES Averaging.action) generate_messages(DEPENDENCIES std_msgs actionlib_msgs)
然后运行:
$ cd %TOP_DIR_YOUR_CATKIN_WORKSPACE% $ catkin_make
编写一个简单的行为服务器
首先,用你最喜欢的编辑器创建actionlib_tutorials/src/averaging_server.cpp,然后用添加以下内容:
代码
1 #include <ros/ros.h>
2 #include <std_msgs/Float32.h>
3 #include <actionlib/server/simple_action_server.h>
4 #include <actionlib_tutorials/AveragingAction.h>
5
6 class AveragingAction
7 {
8 public:
9
10 AveragingAction(std::string name) :
11 as_(nh_, name, false),
12 action_name_(name)
13 {
14 //注册目标和反馈回调函数
15 as_.registerGoalCallback(boost::bind(&AveragingAction::goalCB, this));
16 as_.registerPreemptCallback(boost::bind(&AveragingAction::preemptCB, this));
17
18 //订阅感兴趣的话题数据
19 sub_ = nh_.subscribe("/random_number", 1, &AveragingAction::analysisCB, this);
20 as_.start();
21 }
22
23 ~AveragingAction(void)
24 {
25 }
26
27 void goalCB()
28 {
29 // 重置帮助变量
30 data_count_ = 0;
31 sum_ = 0;
32 sum_sq_ = 0;
33 // 接收新目标
34 goal_ = as_.acceptNewGoal()->samples;
35 }
36
37 void preemptCB()
38 {
39 ROS_INFO("%s: Preempted", action_name_.c_str());
40 // 设置行为状态为抢占(preempted)
41 as_.setPreempted();
42 }
43
44 void analysisCB(const std_msgs::Float32::ConstPtr& msg)
45 {
46 // 确保行为还没有被取消
47 if (!as_.isActive())
48 return;
49
50 data_count_++;
51 feedback_.sample = data_count_;
52 feedback_.data = msg->data;
53 //处理std_dev和数据含义
54 sum_ += msg->data;
55 feedback_.mean = sum_ / data_count_;
56 sum_sq_ += pow(msg->data, 2);
57 feedback_.std_dev = sqrt(fabs((sum_sq_/data_count_) - pow(feedback_.mean, 2)));
58 as_.publishFeedback(feedback_);
59
60 if(data_count_ > goal_)
61 {
62 result_.mean = feedback_.mean;
63 result_.std_dev = feedback_.std_dev;
64
65 if(result_.mean < 5.0)
66 {
67 ROS_INFO("%s: Aborted", action_name_.c_str());
68 //设置行为状态为崩溃(aborted)
69 as_.setAborted(result_);
70 }
71 else
72 {
73 ROS_INFO("%s: Succeeded", action_name_.c_str());
74 // 设置行为状态为成功(succeeded)
75 as_.setSucceeded(result_);
76 }
77 }
78 }
79
80 protected:
81
82 ros::NodeHandle nh_;
83 actionlib::SimpleActionServer<actionlib_tutorials::AveragingAction> as_;
84 std::string action_name_;
85 int data_count_, goal_;
86 float sum_, sum_sq_;
87 actionlib_tutorials::AveragingFeedback feedback_;
88 actionlib_tutorials::AveragingResult result_;
89 ros::Subscriber sub_;
90 };
91
92 int main(int argc, char** argv)
93 {
94 ros::init(argc, argv, "averaging");
95
96 AveragingAction averaging(ros::this_node::getName());
97 ros::spin();
98
99 return 0;
100 }
(查看存储库的代码)
代码解释
现在,让我们分块解释代码。
使用的actionlib/server/simple_action_server.h是行为库,从简单行为实现。
这个包含从以上Averaging.action文件中生成的消息。这个头文件自动从AveragingAction.msg文件生成,更多关于消息定义的信息,查看msg页面。
6 class AveragingAction
7 {
8 public:
9
10 AveragingAction(std::string name) :
11 as_(nh_, name, false),
12 action_name_(name)
13 {
14 //注册目标和反馈回调函数
15 as_.registerGoalCallback(boost::bind(&AveragingAction::goalCB, this));
16 as_.registerPreemptCallback(boost::bind(&AveragingAction::preemptCB, this));
在行为构造函数中,创建一个行为服务器。行为服务器加载一个节点句柄(node handle)、行为名称和可选的回调(executeCB)。在本示例中创建的行为服务器不需要回调(executeCB)参数。在行为服务器构造之后,等价替换成注册的目标(goal)和抢占(preempt)回调用于行为的构造函数。
这里,建立一个数据回调,该回调会处理行为并且行为服务器开启。
这里是一个目标(goalCB)回调函数,参考构造函数。这个回调函数不返回任何东西也不需要任何参数。当目标回调(goalCB)调用行为时,需要接收目标并且存储任何重要的信息。如果你需要在你接收它之前查看目标,查阅SimpleActionServer(ExecuteCallbackMethod) 教程。
行为事件声明,当回调发生,行为代码会运行,否则一个会创建一个抢占回调来保证行为响应及时来取消请求。回调函数不需要参数,并且会设置抢占(preempted)到行为服务器。
这里模拟回调得到订阅数据通道的消息格式,并且在继续处理数据之前检查行为是否处于活跃状态。
50 data_count_++;
51 feedback_.sample = data_count_;
52 feedback_.data = msg->data;
53 //处理std_dev和数据含义
54 sum_ += msg->data;
55 feedback_.mean = sum_ / data_count_;
56 sum_sq_ += pow(msg->data, 2);
57 feedback_.std_dev = sqrt(fabs((sum_sq_/data_count_) - pow(feedback_.mean, 2)));
58 as_.publishFeedback(feedback_);
这里,把相关数据放到反馈变量中,然后在行为服务器提供的反馈通道发布出去。
60 if(data_count_ > goal_)
61 {
62 result_.mean = feedback_.mean;
63 result_.std_dev = feedback_.std_dev;
64
65 if(result_.mean < 5.0)
66 {
67 ROS_INFO("%s: Aborted", action_name_.c_str());
68 //设置行为状态为崩溃(aborted)
69 as_.setAborted(result_);
70 }
71 else
72 {
73 ROS_INFO("%s: Succeeded", action_name_.c_str());
74 // 设置行为状态为成功(succeeded)
75 as_.setSucceeded(result_);
76 }
77 }
78 }
一旦收集到足够的数据,行为服务器会设置成功或失败。这将禁用行为服务器并且analysisCB函数会向之前描述的那样立即返回。
80 protected:
81
82 ros::NodeHandle nh_;
83 actionlib::SimpleActionServer<actionlib_tutorials::AveragingAction> as_;
84 std::string action_name_;
85 int data_count_, goal_;
86 float sum_, sum_sq_;
87 actionlib_tutorials::AveragingFeedback feedback_;
88 actionlib_tutorials::AveragingResult result_;
89 ros::Subscriber sub_;
90 };
这些是行为类的受保护的变量。在构造行为的过程中,构造节点句柄然后传递到行为服务器。构造的行为服务器正如以上描述那样。创建的反馈和结果消息用于在行为中发布。创建的订阅也会保存节点句柄。
最后的main函数,创建行为并且循环节点。行为会运行和等待接收目标。
编译 & 运行行为
在你的CMakeLists.txt文件中添加以下几行:
add_executable(averaging_server src/averaging_server.cpp) target_link_libraries(averaging_server ${catkin_LIBRARIES})
在你使用catkin_make编译之后,在一个新终端开启一个roscore。
$ roscore
然后运行行为服务器:
$ rosrun actionlib_tutorials averaging_server
你会看到类似如下输出:
[ INFO] 1250790662.410962000: Started node [/averaging], pid [29267], bound on [aqy], xmlrpc port [39746], tcpros port [49573], logging to [~/ros/ros/log/averaging_29267.log], using [real] time
检查你的行为运行正常,使用rostopic list命令查看发布情况:
$ rostopic list -v
你会看到类似如下输出:
Published topics: * /averaging/status [actionlib_msgs/GoalStatusArray] 1 publisher * /averaging/result [actionlib_tutorials/AveragingActionResult] 1 publisher * /averaging/feedback [actionlib_tutorials/AveragingActionFeedback] 1 publisher * /rosout [roslib/Log] 1 publisher * /rosout_agg [roslib/Log] 1 publisher Subscribed topics: * /time [unknown type] 2 subscribers * /rosout [roslib/Log] 1 subscriber * /averaging/goal [unknown type] 1 subscriber * /averaging/cancel [unknown type] 1 subscriber
另外你可以查看节点:
$ rosrun rqt_graph rqt_graph &
这里显示了你的行为服务器发布反馈、状态和期望的结果通道,以及订阅目标和期望取消通道。行为服务器运行正常。
发送目标到行为服务器
对于下一步使用你的行为,你需要按下Ctrl-C行为服务器,然后 创建一个简单的行为客户端线程。