实验现象
运行小海龟程序后，向actionserver发送一个坐标，程序会将小海龟移动到该坐标。在移动期间还会不断向客户端发送实时位置。
实验步骤
1.创建工作空间
2.VSCODE集成ROS
笔者使用了VSCode来编写代码，VSCode集成ROS的过程我不自己写，大家可以看看这位博主的博客，写的很好：【ROS】VSCODE + ROS 配置方法（保姆级教程，总结了多篇）
3.创建软件包
在创建好工作空间之后，在src右键选择Create Catkin Package，输入包名后，导入依赖的库：
roscpp rospy std_msgs turtlesim geometry_msgs actionlib actionlib_msgs
4.创建action文件
分析一下程序的过程。服务端订阅话题/turtle1/pose获得小海龟的位姿信息，当客户端发出目的坐标时，服务端根据小海龟的当前位姿信息与目的坐标之间的相对位置，向话题/turtle1/cmd_vel发送控制信息控制小海龟的移动。
4.1 turtlesim/Pose
上文说到，服务端通过话题/turtle1/pose得到小海龟的当前位姿信息，而使用命令：
rostopic info /turtle1/pose
可以知道，这个话题发出的数据类型是turtlesim/Pose，该数据类型有以下五个成员变量：
x：横坐标
y：纵坐标
theta：方向（右边为0，逆时针到左边从0到3.1，顺时针到左边0到-3.1）
linear_velocity：线速度
angular_velocity：角速度
服务器从话题/turtle1/pose拿到一个turtlesim/Pose类型的变量之后，通过x，y，theta三个属性可以知道当前小海龟的位置与朝向。
4.2 geometry_msgs/Twist
小海龟结点根据话题/turtle1/cmd_vel发出的数据，不断设置小海龟的朝向与速度。
而/turtle1/cmd_vel使用的数据类型是geometry_msgs/Twist，有六个成员变量：
geometry_msgs/Vector3 linear
float64 x
float64 y//不使用
float64 z//不使用
geometry_msgs/Vector3 angular
float64 x//不使用
float64 y//不使用
float64 z
前面服务端得到了turtlesim/Pose的当前位置与目的位置，可以计算出合适的朝向与线速度角速度，并将这三条数据封装成geometry_msgs/Twist类型的数据，发送给话题/turtle1/cmd_vel。小海龟结点从这个话题得到数据后，就开始控制小海龟了。
简单的例子
小海龟结点从/turtle1/pose得到一个Pose变量，里面的x为5，y为5，theta为0，表示当前位置为（5,5），朝向为右。
然后客户端发送两个数据9,9，表示目的坐标（9,9）。
服务端拿到目的坐标后，先横向移动，再纵向移动。
横向移动时，先给话题/turtle1/cmd_vel发送0线速度，1角速度，直到小海龟朝向为右；然后根据当前横坐标与目的横坐标的相对位置给话题/turtle1/cmd_vel发送1（或者-1）线速度，0角速度来左右移动。等到横坐标差不多一致后，再给话题/turtle1/cmd_vel发送0线速度，1角速度使小海龟朝上；再根据纵坐标控制向上或者向下，直到到达最终位置。
因此，我们在软件包下新建action目录，并这样创建action文件：
float64 x
float64 y
---
bool res
---
float64 x
float64 y
上面是客户端发送的数据类型，两个浮点数组成坐标；下面是实时返回的坐标信息；中间是最终移动结果。
创建好action后，修改配置文件：
add_action_files(
FILES
action文件名.action
)
generate_messages(
DEPENDENCIES
actionlib_msgs# geometry_msgs# std_msgs
)
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES hw_action
CATKIN_DEPENDS actionlib actionlib_msgs geometry_msgs roscpp rospy std_msgs turtlesim
# DEPENDS system_lib
)
include_directories(
# include
${catkin_INCLUDE_DIRS}
)
然后Ctrl+shift+b，在devel/include/包名/下面编译生成中间文件。
5.编写源文件
客户端
客户端代码如下：
#include "ros/ros.h"
#include "actionlib/client/simple_action_client.h"
#include "hw_action/turtle_controlAction.h"
typedef actionlib::SimpleActionClient<hw_action::turtle_controlAction> Client;
//处理最终结果
void done_cb(const actionlib::SimpleClientGoalState &state, const hw_action::turtle_controlResultConstPtr &result){
if (state.state_ == state.SUCCEEDED)
{
ROS_INFO("最终结果:%d",result.get()->res);
} else {
ROS_INFO("任务失败！");
}
}
//服务已经激活
void active_cb(){
ROS_INFO("服务已经被激活....");
}
//处理连续反馈
void feedback_cb(const hw_action::turtle_controlFeedbackConstPtr &feedback){
ROS_INFO("当前:%.2f,%.2f",feedback.get()->x,feedback.get()->y);
}
int main(int argc, char* argv[]){
//防止乱码
setlocale(LC_ALL,"");
//初始化
ros::init(argc, argv, "control_client");
ros::NodeHandle nh;
Client client(nh,"turtle_controlAction",true);
client.waitForServer();
//发送目的坐标，并在回调函数中处理结果
hw_action::turtle_controlGoal goal;
goal.x = 3.0;
goal.y = 3.0;
client.sendGoal(goal,&done_cb,&active_cb,&feedback_cb);
ros::spin();
return 0;
}
服务端
#include "ros/ros.h"
#include "actionlib/server/simple_action_server.h"
#include "hw_action/turtle_controlAction.h"
#include "turtlesim/Pose.h"
#include "geometry_msgs/Twist.h"
#include "math.h"
#include "std_msgs/String.h"
typedef actionlib::SimpleActionServer<hw_action::turtle_controlAction> Server;
//全局变量，保存小海龟当前位姿信息
turtlesim::Pose now_pose;
//订阅turtle1/pose后的回调函数，不断更新当前位姿
void callback(const turtlesim::Pose::ConstPtr& msg_p){
now_pose = *msg_p.get();
}
//接收到客户端信息后执行这个
void cb(const hw_action::turtle_controlGoalConstPtr &goal,Server* server){
//定义结果信息与回馈信息
hw_action::turtle_controlResult result;
hw_action::turtle_controlFeedback feedback;
//提取目的坐标
float goal_x = goal->x;
float goal_y = goal->y;
//成为/turtle1/cmd_vel的发布者，这样可以发送数据让小海龟结点拿到
ros::NodeHandle nh;
ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel",10);
ros::Rate r(50);
//不同的Twist变量，分别实现水平移动，转向，停止，纵向移动
geometry_msgs::Twist data;
geometry_msgs::Twist data2;
geometry_msgs::Twist pause;
geometry_msgs::Twist data4;
if(now_pose.x!= -1){
//水平方向是前进还是后退
int v = 1 * (goal_x - now_pose.x < 0 ? -1 : 1);
data.linear.x = v;
//竖直方向是前进还是后退
v = 1 * (goal_y - now_pose.y < 0 ? -1 : 1);
data4.linear.x = v;
//旋转的变量
data2.angular.z = 1;
//先开始旋转，大概头朝右时就可以了
ROS_INFO("复位");
while(abs(now_pose.theta) > 0.02) {
//旋转
pub.publish(data2);
//获取位姿反馈
feedback.x = now_pose.x;
feedback.y = now_pose.y;
server->publishFeedback(feedback);
//有没有转到，转到了就停下来
if(abs(now_pose.theta)<= 0.02) pub.publish(pause);
r.sleep();
}
ROS_INFO("开始水平移动");
//开始水平移动，差不多到达目的横坐标就可以了
while(abs(goal_x - now_pose.x) > 0.05){
//水平移动
pub.publish(data);
//反馈实时信息
feedback.x = now_pose.x;
feedback.y = now_pose.y;
server->publishFeedback(feedback);
//差不多了就停下来
if(abs(goal_x - now_pose.x) <= 0.05) {pub.publish(pause);break;}
r.sleep();
}
ROS_INFO("开始转向");
pub.publish(pause);
while(abs(now_pose.theta - 1.55) > 0.02) {
pub.publish(data2);
feedback.x = now_pose.x;
feedback.y = now_pose.y;
server->publishFeedback(feedback);
if(abs(now_pose.theta - 1.55)<= 0.02) pub.publish(pause);
r.sleep();
}
pub.publish(pause);
ROS_INFO("开始纵向移动");
while(abs(goal_y - now_pose.y) > 0.05){
pub.publish(data4);
feedback.x = now_pose.x;
feedback.y = now_pose.y;
server->publishFeedback(feedback);
if(abs(goal_y - now_pose.y) <= 0.05) pub.publish(pause);
r.sleep();
}
pub.publish(pause);
ROS_INFO("成功到达！");
result.res = 1;
server->setSucceeded(result);
}
else{
result.res = 0;
server->setSucceeded(result);
ROS_INFO("获取当前位置失败！");
}
}
int main(int argc, char* argv[]){
//防止乱码
setlocale(LC_ALL,"");
//初始化
ros::init(argc,argv,"control_server");
ros::NodeHandle nh;
now_pose.x = -1;
ros::Subscriber sub = nh.subscribe<turtlesim::Pose>("/turtle1/pose",10,callback);
//开启action服务器
Server server(nh,"turtle_controlAction",boost::bind(&cb,_1,&server),false);
server.start();
ros::spin();
return 0;
}
配置文件
add_executable(客户端名称
src/客户端名称.cpp
)
add_executable(服务端名称
src/服务端名称.cpp
)
target_link_libraries(客户端名称
${catkin_LIBRARIES}
)
target_link_libraries(服务端名称
${catkin_LIBRARIES}
)
6.编译执行
先提前运行小海龟结点，再执行服务器与客户端文件，发现小海龟按照客户端程序中发出的坐标移动。
总结
本次实验学习了ROS中的action通讯，并做了一个简单的小案例，通过控制小海龟的移动来学习。