ROS 中 Action + server + topic 控制turtle定点

#include头文件

• 确定要用的消息类型(头文件）：
  客户端：actionlib/client/simple_action_client.h 、actionlib/client/terminal_state.h 、sstream
  服务端：geometry_msgs/Twist.h 、actionlib/server/simple_action_server.h 、dynamic_reconfigure/server.h 、sstream

• 确定要用到的文件（依赖）类型（头文件）：srv、cfg 、yaml 、action

  • 客户端：action：<包名/action文件名Action.h>
  • 服务端：
    srv: <包名/srv文件名Srv.h>
    cfg:<包名/cfg文件名Config.h>
    action：<包名/action文件名Action.h>

• .确定要用到的消息对象（服务端、客户端）所在的包（头文件）：

  • 相对客户端：
    仅作为action的客户端，头文件包含相应的消息类头文件即可
  • 相对服务端：
    作为反馈：创建并发布feedback 和 result 消息，
    <包名/cfg文件名Config.h>
    作为客户端： 向serverSrv 请求（call） request
    <包名/srv文件名Srv.h>

思路图片

哈哈，上代码！！！

Action: 服务端
server_crt_pkg_node代码（感觉好LOW的）.

#include "ros/ros.h"
#include <geometry_msgs/Twist.h>
#include "server_ctr_pkg/myTestSrv.h"
#include <turtlesim/Pose.h>
#include <dynamic_reconfigure/server.h>
#include <server_ctr_pkg/TutorialsConfig.h>
#include <actionlib/server/simple_action_server.h>
#include <server_ctr_pkg/TargetgoAction.h>
#include "turtlesim_control/turtlesimPidSrv.h"
#include <iostream> 
#include <unistd.h>
#include <cmath>
#include <math.h>

using namespace std;
class PoseControl
{
   
public:
    PoseControl(std::string name):as_(n,name,boost::bind(&PoseControl::execureCB,this,_1),false),action_name_(name)
	{
   
		PidClient = n.serviceClient<turtlesim_control::turtlesimPidSrv>("turtlesimpid_client");
		as_.start(); 
	}
public:
	//控制乌龟向当前点行走
	void execureCB(const server_ctr_pkg::TargetgoGoalConstPtr &goal)
	{
   
		bool success =true;
		//能被2整除说明是成对的X,Y
		float cur_pointX,cur_pointY;
		if((goal->target_list_point.size()%2) == 0)
		{
   
			for (int i = 0; i < goal->target_list_point.size(); i=i+2)       
			{
   
				cur_pointX = goal->target_list_point[i];
				cur_pointY = goal->target_list_point[i+1];
				if (as_.isPreemptRequested() || !ros::ok())
				{
   
					ROS_INFO("%s:Preempted", action_name_.c_str());
					as_.setPreempted();
					success = false;
					break;
				}
				ROS_INFO("The %d point cur point X: %f, Y: %f",i/2,cur_pointX,cur_pointY);

			   //向客户端反馈走到哪个点了
				feedback_.feedback_cur_point.clear();
				feedback_.feedback_cur_point.push_back(cur_pointX);
				feedback_.feedback_cur_point.push_back(cur_pointY);
				as_.publishFeedback(feedback_); //最新的反馈

				//开始向当前点走
				turtlesim_control::turtlesimPidSrv mysrv;
				mysrv.request.x=cur_pointX;
				mysrv.request.y=cur_pointY;
				mysrv.request.th = 0<