ROS通信模式/动作编程（设置收发节点，使小海龟移动到指定位置）

声明：本文转载，自己进行微微改动方便自己观看，有需要可以看原作者点击这里跳转
一、话题、服务模式编程
1.1 创建工作空间

输入以下三条命令：

mkdir -p ~/comm_ws/src
cd ~/comm_ws/src
catkin_init_workspace

编译工作空间，输入以下命令：

cd ..
catkin_make

在bash中注册工作区

echo $ROS_PACKAGE_PATH

输出工作空间路径代表成功
1.1.1 创建ROS工程包

在工作空间中使用catkin_create_pkg命令去创建一个叫comm(通信)的包，这个包依靠std_msgs、roscpp、rospy。

catkin_create_pkg comm std_msgs rospy roscpp

1.1.2 在工作区编译工程包

输入以下命令：

cd ..
catkin_make

1.2 话题模式编程
1.2.1 创建通信的收、发节点

进入工程包目录

cd ~/comm_ws/src/comm

因为我们已经编译过这个工程包了，所以会在comm文件夹下看到CmakeList.txt、package.xml文件和include、src这两个目录。

可以用ls命令查看：

进入src子目录

cd src

1.2.1.1 在src目录中创建一个发布节点

首先创建一个名为talker.cpp的文件

touch talker.cpp
gedit talker.cpp

并输入以下代码：

#include "ros/ros.h"
#include "std_msgs/String.h"
#include <sstream>
int main(int argc, char **argv)
{
  /**
   * The ros::init() function needs to see argc and argv so that it can perform
   * any ROS arguments and name remapping that were provided at the command line. For programmatic
   * remappings you can use a different version of init() which takes remappings
   * directly, but for most command-line programs, passing argc and argv is the easiest
   * way to do it.  The third argument to init() is the name of the node.
   *
   * You must call one of the versions of ros::init() before using any other
   * part of the ROS system.
   */
  ros::init(argc, argv, "talker");
 
  /**
   * NodeHandle is the main access point to communications with the ROS system.
   * The first NodeHandle constructed will fully initialize this node, and the last
   * NodeHandle destructed will close down the node.
   */
  ros::NodeHandle n;
 
  /**
   * The advertise() function is how you tell ROS that you want to
   * publish on a given topic name. This invokes a call to the ROS
   * master node, which keeps a registry of who is publishing and who
   * is subscribing. After this advertise() call is made, the master
   * node will notify anyone who is trying to subscribe to this topic name,
   * and they will in turn negotiate a peer-to-peer connection with this
   * node.  advertise() returns a Publisher object which allows you to
   * publish messages on that topic through a call to publish().  Once
   * all copies of the returned Publisher object are destroyed, the topic
   * will be automatically unadvertised.
   *
   * The second parameter to advertise() is the size of the message queue
   * used for publishing messages.  If messages are published more quickly
   * than we can send them, the number here specifies how many messages to
   * buffer up before throwing some away.
   */
  ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
 
  ros::Rate loop_rate(10);
 
  /**
   * A count of how many messages we have sent. This is used to create
   * a unique string for each message.
   */
  int count = 0;
  while (ros::ok())
  {
    /**
     * This is a message object. You stuff it with data, and then publish it.
     */
    std_msgs::String msg;
 
    std::stringstream ss;
    ss << "hello world " << count;
    msg.data = ss.str();
 
    ROS_INFO("%s", msg.data.c_str());
 
    /**
     * The publish() function is how you send messages. The parameter
     * is the message object. The type of this object must agree with the type
     * given as a template parameter to the advertise<>() call, as was done
     * in the constructor above.
     */
    chatter_pub.publish(msg);
 
    ros::spinOnce();
 
    loop_rate.sleep();
    ++count;
  }
  return 0;
}

点击右上角save保存
1.2.1.2 在src目录中创建一个订阅节点

首先创建一个listener.cpp文件

touch listener.cpp
gedit listener.cpp

打开文件并输入以下代码：

#include "ros/ros.h"
#include "std_msgs/String.h"
 
/**
 * This tutorial demonstrates simple receipt of messages over the ROS system.
 */
void chatterCallback(const std_msgs::String::ConstPtr& msg)
{
  ROS_INFO("I heard: [%s]", msg->data.c_str());
}
 
int main(int argc, char **argv)
{
  /**
   * The ros::init() function needs to see argc and argv so that it can perform
   * any ROS arguments and name remapping that were provided at the command line. For programmatic
   * remappings you can use a different version of init() which takes remappings
   * directly, but for most command-line programs, passing argc and argv is the easiest
   * way to do it.  The third argument to init() is the name of the node.
   *
   * You must call one of the versions of ros::init() before using any other
   * part of the ROS system.
   */
  ros::init(argc, argv, "listener");
 
  /**
   * NodeHandle is the main access point to communications with the ROS system.
   * The first NodeHandle constructed will fully initialize this node, and the last
   * NodeHandle destructed will close down the node.
   */
  ros::NodeHandle n;
 
  /**
   * The subscribe() call is how you tell ROS that you want to receive messages
   * on a given topic.  This invokes a call to the ROS
   * master node, which keeps a registry of who is publishing and who
   * is subscribing.  Messages are passed to a callback function, here
   * called chatterCallback.  subscribe() returns a Subscriber object that you
   * must hold on to until you want to unsubscribe.  When all copies of the Subscriber
   * object go out of scope, this callback will automatically be unsubscribed from
   * this topic.
   *
   * The second parameter to the subscribe() function is the size of the message
   * queue.  If messages are arriving faster than they are being processed, this
   * is the number of messages that will be buffered up before beginning to throw
   * away the oldest ones.
   */
  ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);
 
  /**
   * ros::spin() will enter a loop, pumping callbacks.  With this version, all
   * callbacks will be called from within this thread (the main one).  ros::spin()
   * will exit when Ctrl-C is pressed, or the node is shutdown by the master.
   */
  ros::spin();
 
  return 0;
}

点击save保存
1.2.2 编辑Cmakelist.txt文件（注意：是comm项目包下的CMakelist文件）

gedit CMakeList.txt

在文件末尾输入以下代码：

include_directories(include ${catkin_INCLUDE_DIRS})
add_executable(talker src/talker.cpp)
target_link_libraries(talker ${catkin_LIBRARIES})
add_dependencies(talker comm_generate_messages_cpp)
add_executable(listener src/listener.cpp)
target_link_libraries(listener ${catkin_LIBRARIES})
add_dependencies(listener comm_generate_messages_cpp)

1.2.3 将目录切换到工作区目录，并执行catkin_make运行命令：

cd ~/comm_ws
catkin_make

1.2.4 测试程序正确性

新开一个终端，启动ROS核心程序

roscore

在原终端注册程序

cd ~/comm_ws
source ./devel/setup.bash

（暂时不要运行这句命令）运行talker节点

rosrun comm talker

新建一个终端运行listener节点（暂时不要运行最后一句命令）

cd ~/comm_ws
source ./devel/setup.bash
rosrun comm listener

准备完成后，先在第一个终端运行talker程序，再在新终端运行listener程序，效果如下：

这表示订阅节点成功接收到了发送节点的信息
可通过CTRL+C结束程序
1.3 服务模式编程
1.3.1 定义服务请求与应答的方式

进入工作空间，定义srv文件

cd comm
mkdir srv
vim AddTwoInts.srv

输入以下代码：

int64 a
int64 b
---
int64 sum

在package.xml中添加功能包依赖

cd ~/comm_ws/src/comm
gedit package.xml

输入以下代码：

<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>

修改CMakeLists.txt

gedit CMakeLists.txt

1.3.2 代码编写

进入comm的src子目录

touch server.cpp
touch client.cpp

gedit server.cpp

输入以下代码

#include<ros/ros.h>
#include"comm/AddTwoInts.h"
//service回调函数，输入参数req，输出参数res
bool add(comm::AddTwoInts::Request &req,comm::AddTwoInts::Response &res)
{
	//将输入的参数中的请求数据相加，结果放到应答变量中
	res.sum=req.a+req.b;
	ROS_INFO("request: x=%1d,y=%1d",(long int)req.a,(long int)req.b);
	ROS_INFO("sending back response:[%1d]",(long int)res.sum);
	return true;
}
int main(int argc,char **argv)
{
	//ROS节点初始化
	ros::init(argc,argv,"add_two_ints_server");
	//创建节点句柄
	ros::NodeHandle n;
	//创建一个名为add_two_ints的server,注册回调函数add()
	ros::ServiceServer service=n.advertiseService("add_two_ints",add);
	//循环等待回调函数
	ROS_INFO("Ready to add two ints.");
	ros::spin();
	return 0;
}

gedit client.cpp

输入以下代码：

#include<cstdlib>
#include<ros/ros.h>
#include"comm/AddTwoInts.h"
int main(int argc,char **argv)
{
	//ROS节点初始化
	ros::init(argc,argv,"add_two_ints_client");
	//从终端命令行获取两个加数
	if(argc!=3)
	{
		ROS_INFO("usage:add_two_ints_client X Y");
		return 1;
	}
	//创建节点句柄
	ros::NodeHandle n;
	//创建一个client，请求add_two_ints_service
	//service消息类型是learning_communication::AddTwoInts
	ros::ServiceClient client=n.serviceClient<comm::AddTwoInts>("add_two_ints");
	//创建learning_communication::AddTwoInts类型的service消息
	comm::AddTwoInts srv;
	srv.request.a=atoll(argv[1]);
	srv.request.b=atoll(argv[2]);
	//发布service请求，等待加法运算的应答请求
	if(client.call(srv))
	{
		ROS_INFO("sum: %1d",(long int)srv.response.sum);
	}
	else
	{
		ROS_INFO("Failed to call service add_two_ints");
		return 1;
	}
	return 0;
}

设置CMakeLists.txt文件

add_executable(server src/server.cpp)
target_link_libraries(server ${catkin_LIBRARIES})
add_dependencies(server ${PROJECT_NAME}_gencpp)
add_executable(client src/client.cpp)
target_link_libraries(client ${catkin_LIBRARIES})
add_dependencies(client ${PROJECT_NAME}_gencpp)

编译

catkin_make
```![在这里插入图片描述](https://img-blog.csdnimg.cn/direct/929e61ba569344ab98d3bf8a8efa4109.png)运行程序：
新开一个终端：

```powershell
roscore

原终端：

source devel/setup.bash
rosrun comm server

再新开一个终端

source devel/setup.bash
rosrun comm client

在客户端输入：

rosrun client server 整数 整数

运行成功！
二、ROS编程，使小海龟移动到指定位置

进入到工程包的src子目录下

cd ~/comm_ws/src/comm/src

2.1 新建服务文件turtleMove.cpp

touch turtleMove.cpp

/*  
　　　此程序通过通过动作编程实现由client发布一个目标位置
　　　然后控制Turtle运动到目标位置的过程
 */
#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include "comm/turtleMoveAction.h"
#include <turtlesim/Pose.h> 
#include <turtlesim/Spawn.h>
#include <geometry_msgs/Twist.h>
 
typedef actionlib::SimpleActionServer<comm::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 comm::turtleMoveGoalConstPtr& goal, Server* as)
{
    comm::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));
        turtle_vel.publish(vel_msgs);
 
        feedback.present_turtle_x=turtle_original_pose.x;
        feedback.present_turtle_y=turtle_original_pose.y;
        feedback.present_turtle_theta=turtle_original_pose.theta;
        as->publishFeedback(feedback);
        ROS_INFO("break_flag=%f",break_flag);
        if(break_flag<0.1) break;
        r.sleep();
    }
        // 当action完成后，向客户端返回结果
        ROS_INFO("TurtleMove is finished.");
        as->setSucceeded();
}
 
int main(int argc, char** argv)
{
    ros::init(argc, argv, "turtleMove");
    ros::NodeHandle n,turtle_node;
    ros::Subscriber sub = turtle_node.subscribe("turtle1/pose",10,&posecallback); //订阅小乌龟的位置信息
    turtle_vel = turtle_node.advertise<geometry_msgs::Twist>("turtle1/cmd_vel",10);//发布控制小乌龟运动的速度
    // 定义一个服务器
        Server server(n, "turtleMove", boost::bind(&execute, _1, &server), false);
        // 服务器开始运行
        server.start();
        ROS_INFO("server has started.");
    ros::spin();
 
    return 0;
}

2.2 创建小乌龟“发布目标位置文件”turtleMoveClient.cpp

输入以下命令：

touch turtleMoveClient.cpp

打开此文件并输入以下代码

gedit turtleMoveClient.cpp

#include <actionlib/client/simple_action_client.h>
#include "comm/turtleMoveAction.h"
#include <turtlesim/Pose.h> 
#include <turtlesim/Spawn.h>
#include <geometry_msgs/Twist.h> 
typedef actionlib::SimpleActionClient<comm::turtleMoveAction> Client;
struct Myturtle
{
    float x;
    float y;
    float theta;
}turtle_present_pose;
 
// 当action完成后会调用该回调函数一次
void doneCb(const actionlib::SimpleClientGoalState& state,
        const comm::turtleMoveResultConstPtr& result)
{
    ROS_INFO("Yay! The turtleMove is finished!");
    ros::shutdown();
}
 
// 当action激活后会调用该回调函数一次
void activeCb()
{
    ROS_INFO("Goal just went active");
}
 
// 收到feedback后调用该回调函数
void feedbackCb(const comm::turtleMoveFeedbackConstPtr& feedback)
{
    ROS_INFO(" present_pose : %f  %f  %f", feedback->present_turtle_x,
                   feedback->present_turtle_y,feedback->present_turtle_theta);
}
 
int main(int argc, char** argv)
{
    ros::init(argc, argv, "turtleMoveClient");
 
    // 定义一个客户端
    Client client("turtleMove", true);
 
    // 等待服务器端
    ROS_INFO("Waiting for action server to start.");
    client.waitForServer();
    ROS_INFO("Action server started, sending goal.");
 
    // 创建一个action的goal
    comm::turtleMoveGoal goal;
    goal.turtle_target_x = 1;
    goal.turtle_target_y = 1;
    goal.turtle_target_theta = 0;
 
    // 发送action的goal给服务器端，并且设置回调函数
    client.sendGoal(goal,  &doneCb, &activeCb, &feedbackCb);
 
    ros::spin();
 
    return 0;
}

2.3 在功能包目录下创建action文件夹

输入以下命令:

cd ..
mkdir action

进入action文件夹并创建turtleMove.action文件

cd action
touch turtleMove.action

打开此文件并输入以下代码

action是动作编程里的处理代码，相当于servlet，一个用户的请求发过来，交个action处理，处理完了返回需要返回的界面，其实就是一个自己写的类，让这个类处理，这个类里有很多方法，交给你指定的方法处理
2.4 修改CMakeLists.txt文件内容

进入工程包目录

cd ..

打开CMakeLists.txt文件

sudo gedit CMakeLists.txt

在文件末尾输入以下代码

add_executable(turtleMoveClient src/turtleMoveClient.cpp)
target_link_libraries(turtleMoveClient ${catkin_LIBRARIES})
add_dependencies(turtleMoveClient ${PROJECT_NAME}_gencpp)
add_executable(turtleMove src/turtleMove.cpp)
target_link_libraries(turtleMove ${catkin_LIBRARIES})
add_dependencies(turtleMove ${PROJECT_NAME}_gencpp)

在该文件中找到find_package函数方法，修改为如下：

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  message_generation
  actionlib_msgs
  actionlib
)

继续在该文件中找到add_action_files函数一项，默认是用#注释掉了的，这里我们找到后修改为如下代码：

 add_action_files(
   FILES
   turtleMove.action
 )

注意：以上turtleMove.action为我们在action文件夹下面创建的文件名字turtleMove.action

继续在该文件中找到generate_messages函数一项，默认也是#注释，这里修改为如下代码：

generate_messages(
   DEPENDENCIES
   std_msgs
   actionlib_msgs
 )

找到catkin_package函数一项，修改为如下代码：

#  INCLUDE_DIRS include
#  LIBRARIES comm
#  CATKIN_DEPENDS roscpp rospy std_msgs
#  DEPENDS system_lib
 CATKIN_DEPENDS roscpp rospy std_msgs
  message_runtime
)

保存关闭文件
2.5 修改package.xml文件内容

gedit package.xml

将文件中build_depend一栏、替换为如下代码：

<buildtool_depend>catkin</buildtool_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>rospy</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_depend>message_generation</build_depend>
  <build_depend>actionlib</build_depend>
  <build_depend>actionlib_msgs</build_depend>
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>rospy</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>

将文件中exec_depend一栏、替换为如下代码：

<exec_depend>roscpp</exec_depend>
  <exec_depend>rospy</exec_depend>
  <exec_depend>std_msgs</exec_depend>
  <exec_depend>message_runtime</exec_depend>
  <exec_depend>actionlib</exec_depend>
  <exec_depend>actionlib_msgs</exec_depend> 

保存关闭
2.6 运行程序进行测试
2.6.1 进入工作空间

cd ~/comm_ws
catkin_make

编译成功

注册程序

source ./devel/setup.bash

不要关闭此终端（终端1），输入以下命令但先不要运行

rosrun comm turtleMove

2.6.2 新建一个终端，启动ros核心程序

roscore

2.6.3 再新建一终端（终端3），运行小海龟

rosrun turtlesim turtlesim_node

2.6.4 再新建一个终端(终端4)运行目标位置程序代码：

cd ~/comm_ws
source ./devel/setup.bash

输入以下命令，但先不要运行

rosrun comm turtleMoveClient

2.6.5 开始测试

终端1回车、终端4回车，出现如下画面，则我们动作编程的实验完美成功

终端4在实时更新小乌龟的位置，而运行窗口则显示小乌龟的移动路径