ROS通信模式编程

一、话题、服务模式的ROS程序设计

1、在工程包中创建项动作编程需要的文件
创建小乌龟移动的“服务文件”turtleMove.cpp
（1）新建一个终端，这里命名为终端 1,然后进入工程包 my_turtle_package下的 src 文件中
命令：cd ~/catkin_ws/src/my_turtle_package/src
2) 新建服务文件 turtleMove.cpp
命令：touch turtleMove.cpp
3) 编写代码：将如下 C++语言代码写入 turtleMove.cpp 文件中，保存后关闭！
命令：gedit turtleMove.cpp

/*
本程序是为了实现一个为ROS的动作编程，通过本程序是实现client发布一个位置信息给控制海龟移动的turtlel程序，使海龟移动到目标位置
*/
#include<ros/ros.h>
#include<actionlib/server/simple_action_server.h>
#include"my_turtle_package/turtleMoveAction.h"
#include<turtlesim/Pose.h>
#include<turtlesim/Spawn.h>
#include<geometry_msgs/Twist.h>
typedef actionlib::SimpleActionServer<my_turtle_package::turtleMoveAction> Server;

struct Myturtle
{
   
	float x;
	float y;
	float theta;
}turtle_original_pose,turtle_target_pose;
ros::Publisher turtle_vel;
void posecallback(const turtlesim::PoseConstPtr &msg)
{
   
ROS_INFO("turtle1_position:(%f,%f,%f)",msg->x,msg->y,msg->theta);
turtle_original_pose.x=msg->x;
turtle_original_pose.y=msg->y;
turtle_original_pose.theta=msg->theta;
}
// 收到 action 的 goal 后调用该回调函数
void execute(const my_turtle_package::turtleMoveGoalConstPtr &goal, Server* as)
{
   
my_turtle_package::turtleMoveFeedback feedback;
ROS_INFO("TurtleMove is working.");
turtle_target_pose.x=goal->turtle_target_x;
turtle_target_pose.y=goal->turtle_target_y;
turtle_target_pose.theta=goal->turtle_target_theta;
geometry_msgs::Twist vel_msgs;
float break_flag;
	while(1)
	{
   
		ros::Rate r(10);
		vel_msgs.angular.z = 4.0 *(atan2(turtle_target_pose.y-turtle_original_pose.y,
	turtle_target_pose.x-turtle_original_pose.x)-turtle_original_pose.theta);
	vel_msgs.linear.x = 0.5 *sqrt(pow(turtle_target_pose.x-turtle_original_pose.x, 2) +pow(turtle_target_pose.y-turtle_original_pose.y, 2));
	break_flag=sqrt(pow(turtle_target_pose.x-turtle_original_pose.x, 2)+pow(turtle_target_pose.y-turtle_original_pose.y, 2))