ROS-ActionServerClient
actionlib tutorial 参考连接：http://wiki.ros.org/actionlib_tutorials/Tutorials
功能说明
创建一个计算fibonacci数列的action Server
- goal 是输入一个整数，为需要计算的第几个fibonacci数
- result是一个整型数组：数组存放所有的fibonacci数；
- feedback是一个整型数组：数组依次存放在计算过程中的fibonacci数；
00. 如何构建ActionServer
0.1 创建action文件
在工程目录中，创建action目录，然后创建action文件，内容如下
wiki_actionlib_tutorials/action/Fibonacci.action
#goal definition
int32 order
---
#result definition
int32[] sequence
---
#feedback
int32[] sequence
0.2 修改CMakeLists.txt文件
find_package(catkin REQUIRED COMPONENTS roscpp actionlib actionlib_msgs)
## 需要包含actionlib_msgs actionlib
add_action_files(
DIRECTORY action
FILES Fibonacci.action )
## 关联action目录中的action文件，前面创建好的
generate_messages(
DEPENDENCIES
actionlib_msgs std_msgs )
catkin_package(
CATKIN_DEPENDS actionlib_msgs )
0.3 package.xml
依赖actionlib包：
<build_depend>actionlib</build_depend>
<build_depend>actionlib_msgs</build_depend>
<build_export_depend>actionlib</build_export_depend>
<build_export_depend>actionlib_msgs</build_export_depend>
<exec_depend>actionlib</exec_depend>
<exec_depend>actionlib_msgs</exec_depend>
catkin_make
编译成功后，
~/catkin_ws/devel/share/wiki_actionlib_tutorials/msg 目录中
~/catkin_ws/devel/include/wiki_actionlib_tutorials 目录中：
01.Action 类方式实现
1.1 Server
使用类方式的形式，实现Action Server，并启动Server
#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include <wiki_actionlib_tutorials/FibonacciAction.h>
class FibonacciAction
{
protected:
ros::NodeHandle nh_;
actionlib::SimpleActionServer<wiki_actionlib_tutorials::FibonacciAction> as_; // NodeHandle instance must be created before this line. Otherwise strange error occurs.
std::string action_name_;
// create messages that are used to published feedback/result
wiki_actionlib_tutorials::FibonacciFeedback feedback_;
wiki_actionlib_tutorials::FibonacciResult result_;
public:
FibonacciAction(std::string name) : as_(nh_, name, boost::bind(&FibonacciAction::executeCB, this, _1), false),action_name_(name)
{
as_.start();
ROS_INFO_STREAM(action_name_ << ": server start!");
}
~FibonacciAction(void)
{
}
void executeCB(const wiki_actionlib_tutorials::FibonacciGoalConstPtr &goal)
{
// helper variables
ros::Rate r(0.5);
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::ok())
{
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
ROS_INFO_STREAM(action_name_ << ": current index " << i);
for (int j = 0; j != feedback_.sequence.size(); j++)
{
std::cout << feedback_.sequence[j] << ".";
}
std::cout << std::endl;
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("fibonacci");
ros::spin();
return 0;
}
as_(nh_, name, boost::bind(&FibonacciAction::executeCB, this, _1), false)对于actionServer的创建，第三个参数要绑定到类中的成员函数，区别于普通函数，需要有几个注意事项；- boost::bind()绑定类成员函数的时候，需要在成员函数前使用“&”
- 由于类成员函数的调用，需要知道是在调用哪一个类实例的成员函数，所以需要传入类实例，所以此处boost::bind()的第二个参数是类实例，由于这是在类中进行绑定的，所以第二个参数可以写成是this；
- boost::bind()的第三个参数：_1，是留给后续ActionGoal消息传进来的时候，占位符，由于实际的参数值是未知的，所以使用占位符；
- 说明
rosrun wiki_actionlib_tutorials fibonacci_server
启动server，此时将server初始化并start了，但是由于没有收到goal的消息，所以处于等待状态
rostopic list -v
Published topics:
* /rosout_agg [rosgraph_msgs/Log] 1 publisher
* /rosout [rosgraph_msgs/Log] 1 publisher
* /fibonacci/result [wiki_actionlib_tutorials/FibonacciActionResult] 1 publisher
* /fibonacci/feedback [wiki_actionlib_tutorials/FibonacciActionFeedback] 1 publisher
* /fibonacci/status [actionlib_msgs/GoalStatusArray] 1 publisher
Subscribed topics:
* /rosout [rosgraph_msgs/Log] 1 subscriber
* /fibonacci/goal [wiki_actionlib_tutorials/FibonacciActionGoal] 1 subscriber
* /fibonacci/cancel [actionlib_msgs/GoalID] 1 subscriber
- 通过/fibonacci/goal 向ActionServer发送执行目标；
- 通过/fibonacci/cancel 向server请求取消任务，此时server会检测到as_.isPreemptRequested()，从而结束
- /fibonacci/result 只有在server完成全部工作的时候，才会向这个topic发送一次FibonacciActionResult消息
- /fibonacci/feedback 会按照程序中指定的频率，反馈 FibonacciActionFeedback消息
- /fibonacci/status 会按照5hz的频率，固定发布当前server的状态数据
## 向server发送FibonacciActionGoal消息
rostopic pub -1 /fibonacci/goal wiki_actionlib_tutorials/FibonacciActionGoal "header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: ''
goal_id:
stamp:
secs: 0
nsecs: 0
id: ''
goal:
order: 20"
1.2 Client
如下，当client执行的时候，会向Action Server发送一个请求ac.sendGoal(goal)，server收到请求后，开始运行；
#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <actionlib/client/terminal_state.h>
#include <wiki_actionlib_tutorials/FibonacciAction.h>
int main(int argc, char **argv)
{
ros::init(argc, argv, "fibonacci_client");
actionlib::SimpleActionClient<wiki_actionlib_tutorials::FibonacciAction> ac("fibonacci", true);
ROS_INFO("Waiting for action server to start.");
ac.waitForServer(); // will wait for infinite time
ROS_INFO("Action server started, sending goal.");
wiki_actionlib_tutorials::FibonacciGoal goal;
goal.order = 10;
ac.sendGoal(goal);
bool finished_before_timeout = ac.waitForResult(ros::Duration(30.0));
if (finished_before_timeout)
{
actionlib::SimpleClientGoalState state = ac.getState();
ROS_INFO("Action finished: %s", state.toString().c_str());
}
else
ROS_INFO("Action did not finish before the time out.");
return 0;
}
02.Action 普通的函数方式实现
2.1 Server
#include <chores/DoDishesAction.h> // Note: "Action" is appended
#include <actionlib/server/simple_action_server.h>
typedef actionlib::SimpleActionServer<chores::DoDishesAction> Server;
void execute(const chores::DoDishesGoalConstPtr& goal, Server* as) // Note: "Action" is not appended to DoDishes here
{
// Do lots of awesome groundbreaking robot stuff here
as->setSucceeded();
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "do_dishes_server");
ros::NodeHandle n;
Server server(n, "do_dishes", boost::bind(execute, _1, &server), false);//boost::bind()绑定函数需要此处需要注意，需要理解下
server.start();
ros::spin();
return 0;
}
说明：
- 首先创建一个Server，且在server中绑定会到函数，在server启动之后，在回调函数中对收到的goal执行响应的操作
- 在示例代码中，没有写详细的处理过程，应该在这里有按照一定的频率反馈feekback的操作
- 在回调函数中，还应该检测server的状态，检测是否被强占了，这是应该及时处理，结束server；
Server server(n, "do_dishes", boost::bind(execute, _1, &server), false);此处ActionServer的创建，首先需要注意其标准规范；其次，server()的第三个参数是actionServer在收到goal消息之后，进行消息处理的回调函数execute()，- 回调函数：execute()含有两个参数，分别是ActionGoal消息和一个自定义的参数server，第一个参数是必须的，用来接收actionGoal，第二个是用户自定的，非必需；
- 绑定回调函数：
boost::bind(execute, _1,&server),对于要绑定的普通函数，没有必要一定要使用&,_1表示当接受到ActionGoal消息的时候，会传给占位符_1这个位置，&server表示要传给execute的第二个参数，用于在函数中对该server进行一些状态操作，此处server就是本声明语句要创建的对象，这样使用，证明是可以的。
2.2 Client
#include <chores/DoDishesAction.h> // Note: "Action" is appended
#include <actionlib/client/simple_action_client.h>
typedef actionlib::SimpleActionClient<chores::DoDishesAction> Client;
int main(int argc, char** argv)
{
ros::init(argc, argv, "do_dishes_client");
Client client("do_dishes", true); // true -> don't need ros::spin()
client.waitForServer();
chores::DoDishesGoal goal;
// Fill in goal here
client.sendGoal(goal);
client.waitForResult(ros::Duration(5.0));
if (client.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
printf("Yay! The dishes are now clean");
printf("Current State: %s\n", client.getState().toString().c_str());
return 0;
}
浙公网安备 33010602011771号