Experimental phenomena
After running the little turtle program, send a coordinate to actionserver, and the program will move the little turtle to this coordinate. The real-time location is also continuously sent to the client during the movement.
Experimental procedure
1. Create a workspace
2. VSCODE integrates ROS
The author uses VSCode to write the code. I don’t write the process of VSCode integrating ROS by myself. You can take a look at this blogger’s blog. article)
3. Create package
After creating the workspace, right click on src and select Create Catkin Package, enter the package name, and import the dependent library:
roscpp rospy std_msgs turtlesim geometry_msgs actionlib actionlib_msgs
4. Create action file
Analyze the process of the program. The server subscribes to the topic /turtle1/pose to obtain the pose information of the turtle. When the client sends the target coordinates, the server sends a message to the topic /turtle1/cmd_vel according to the relative position between the current pose information of the turtle and the target coordinates. The control information controls the movement of the little turtle.
4.1 turtlesim/Pose
As mentioned above, the server obtains the current pose information of the little turtle through the topic /turtle1/pose, and uses the command:
rostopic info /turtle1/pose
It can be known that the data type sent by this topic is turtlesim/Pose, which has the following five member variables:
x：横坐标
y：纵坐标
theta：方向（右边为0，逆时针到左边从0到3.1，顺时针到左边0到-3.1）
linear_velocity：线速度
angular_velocity：角速度
After the server gets a variable of the turtlesim/Pose type from the topic /turtle1/pose, the current position and orientation of the little turtle can be known through the three attributes of x, y, and theta.
4.2 geometry_msgs/Twist
The little turtle node continuously sets the direction and speed of the little turtle according to the data sent by the topic /turtle1/cmd_vel.
The data type used by /turtle1/cmd_vel is geometry_msgs/Twist, which has six member variables:
geometry_msgs/Vector3 linear
float64 x
float64 y//不使用
float64 z//不使用
geometry_msgs/Vector3 angular
float64 x//不使用
float64 y//不使用
float64 z
The previous server has obtained the current position and target position of turtlesim/Pose, can calculate the appropriate orientation and linear velocity and angular velocity, and encapsulate these three pieces of data into geometry_msgs/Twist type data, and send it to topic /turtle1/cmd_vel. After the little turtle node gets the data from this topic, it starts to control the little turtle.
simple example
The little turtle node gets a Pose variable from /turtle1/pose, in which x is 5, y is 5, and theta is 0, indicating that the current position is (5,5) and the orientation is right.
Then the client sends two data 9,9, indicating the destination coordinates (9,9).
After the server gets the target coordinates, it first moves horizontally and then vertically.
When moving horizontally, first send 0 linear velocity and 1 angular velocity to topic /turtle1/cmd_vel until the little turtle is facing right; then send 1 (or -1 ) linear velocity, and 0 angular velocity to move left and right. Wait until the abscissa is almost the same, then send the topic /turtle1/cmd_vel a linear velocity of 0 and an angular velocity of 1 to make the little turtle face up; then control the upward or downward according to the ordinate until it reaches the final position.
Therefore, we create a new action directory under the package and create an action file like this:
float64 x
float64 y
---
bool res
---
float64 x
float64 y
The above is the data type sent by the client, and two floating-point numbers form the coordinates; the bottom is the coordinate information returned in real time; the middle is the final movement result.
After creating the action, modify the configuration file:
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}
)
Then Ctrl+shift+b, compile and generate intermediate files under devel/include/package name/.
5. Write source files
client
The client code is as follows:
#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;
}
Server
#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;
}
configuration file
add_executable(客户端名称
src/客户端名称.cpp
)
add_executable(服务端名称
src/服务端名称.cpp
)
target_link_libraries(客户端名称
${catkin_LIBRARIES}
)
target_link_libraries(服务端名称
${catkin_LIBRARIES}
)
6. Compile and execute
First run the little turtle node in advance, and then execute the server and client files, and find that the little turtle moves according to the coordinates sent by the client program.
Summarize
In this experiment, I learned the action communication in ROS, and made a simple small case to learn by controlling the movement of the little turtle.