ROS | 话题通信的编程实现

1. 创建功能包

在ROS工作空间ROS_ws的src文件夹目录下创建一个功能包，命名为test_pkg，并编译完成。

2. 节点编程与消息定义

2.1 案例说明

定义一个发布者Publisher，通过自定义的消息Person将消息数据name、sex及age等发送到订阅者Subscriber，同时订阅者将消息内容打印到终端界面。

2.2 话题消息的定义

在功能包目录下创建一个新的文件夹，命名为msg，并在此文件夹中创建一个空文件Person.msg。可以使用鼠标右键New Document>Empty Document，或在src路径下通过终端命令符touch创建空文件。

在Person.msg文件中输入以下代码，定义话题消息。

string name
uint8 sex
uint8 age  
	
uint8 unknown = 0
uint8 male    = 1
uint8 female  = 2

2.3 创建.cpp文件

在功能包下面的src文件夹目录下创建一个空文件Publisher_test.cpp。

2.4 话题发布者编程

打开上述创建的文件Publisher_test，输入以下代码。

#include "ros/ros.h"
#include "test_pkg/Person.h"
	
int main(int argc, char **argv)
{
	//初始化ROS节点
	ros::init(argc, argv, "Publisher_test");
	
	//创建句柄
	ros::NodeHandle n;
	
	//创建一个发布者Publisher，发布名为/person_info的topic，消息类型为test_pkg::Person，队列长度为10
	ros::Publisher person_info_pub = n.advertise<test_pkg::Person>("/person_info", 10);
	
	//设置消息发布频率
	ros::Rate loop_rate(1);
	
	int count = 0;
	while(ros::ok())
	{
		//初始化消息内容
		test_pkg::Person person_msg;
		person_msg.name = "Tom";
		person_msg.age  = 18;
		person_msg.sex  = test_pkg::Person::male;
	
		//发布消息
		person_info_pub.publish(person_msg);
		
		ROS_INFO("Publish Person Info: name:%s  age:%d  sex:%d",
			  person_msg.name.c_str(), person_msg.age, person_msg.sex);
	
		//按照设置的频率延时
		loop_rate.sleep();
	
	}
}

说明：

• 头文件ros/ros.h包含了标准ROS类的声明，在每一个ROS程序中都需要包含它。
• 头文件test_pkg/Person.h是由Person.msg编译扩展得到，包含了针对C++类的定义等，它存放在工作空间的devel文件夹中。
• main函数中ros::init(argc, argv, “Publisher_test”)的作用是初始化ROS节点，第三个参数表示节点名称，这个节点名是唯一的。
• ros::NodeHandle n;的作用是创建句柄，启动ROS节点。
• ros::Publisher person_info_pub = n.advertise<test_pkg::Person>(“/person_info”, 10);使用ros::Publisher类定义了一个发布者对象person_info_pub，发布的消息的数据类型为test_pkg::Person，用<>表示。/person_info表示发布消息数据的话题名，必须要与订阅者订阅的话题名相同。advertise()的第二个参数表示用于发布消息的消息队列长度，如果发布消息的速度快于底层响应的速度，则此处的数字指定在丢弃一些消息之前要缓冲多少条最新的消息。调用advertise()函数后，主节点（ROS Master）将通知任何试图订阅此话题名的节点，然后他们将与此节点协商对接。同时advertise()将返回一个Publisher对象，该对象使您可以通过调用publish()来发布有关该主题的消息。
• ros::Rate loop_rate(1);的作用是设置消息发布频率为1Hz。
• int count = 0;表明我们已发送多少条消息，用于为每个消息创建一个唯一的字符串。
• 若ros::ok()返回false，则可能发生了以下事件：
  (1) SIGINT被触发(Ctrl-C)；
  (2) 被另一同名节点踢出 ROS 网络；
  (3) ros::shutdown()被程序的另一部分调用；
  (4) 节点中的所有ros::NodeHandles 都已经被销毁。

test_pkg::Person person_msg;
person_msg.name = "Tom";
person_msg.age  = 18;
person_msg.sex  = test_pkg::Person::male;

• 这部分的作用是对消息内容初始化，首先创建一个test_pkg::Person类的对象，之后设置这个对象中的name、age和sex。
• person_info_pub.publish(person_msg);的作用是发布消息person_msg。
• ROS_INFO()类似于C语言中的printf()函数，将内容打印在终端界面显示。
• loop_rate.sleep();的作用是延时，以保证消息能够按照之前设置的发布频率发布出去。

综上所述，实现一个话题发布者的步骤大致可分为以下几点：

1. 初始化ROS节点；
2. 向ROS Master注册节点信息，包括发布的话题名和话题中的消息类型；
3. 按照一定频率循环发送消息。

2.5 话题订阅者编程

在src文件夹下再创建一个空文件Subscriber_test.cpp，输入以下代码。

#include "ros/ros.h"
#include "test_pkg/Person.h"
	
//收到消息时进入回调函数
void personInfoCallback(const test_pkg::Person::ConstPtr& msg)
{
	//打印消息
	ROS_INFO("Subscribe Person Info: name:%s  age:%d  sex:%d",
		      msg->name.c_str(), msg->age, msg->sex);
}
	
int main(int argc, char **argv)
{
	//初始化ROS节点
	ros::init(argc, argv, "Subscriber_test");
	
	//创建句柄
	ros::NodeHandle n;
	
	//创建一个订阅者Subscriber
	ros::Subscriber person_info_sub = n.subscribe("/person_info", 10, personInfoCallback);
	
	//循环接收消息
	ros::spin();
	
	return 0;
}

说明：

• personInfoCallback(const test_pkg::Person::ConstPtr& msg)是一个回调函数，一旦订阅者接收到消息，则执行该回调函数。在该回调函数中，打印消息内容name、age和sex。
• main函数中一开始跟发布者一样，初始化ROS节点，创建句柄。
• ros::Subscriber person_info_sub = n.subscribe(“/person_info”, 10, personInfoCallback);使用ros::Subscriber类定义一个订阅者对象person_info_sub。subscribe()函数中的第一个参数表示需要订阅的话题名，第二个参数表示消息队列的长度，如果消息到达的速度快于处理速度，则开始丢弃最旧的消息之前将被缓冲的消息数。subscribe()函数的第三个参数是回调函数名，消息被传递到回调函数。
• ros::spin()在调用后不会再返回，也就是主程序到这儿就不往下执行了，一般放在main函数最后，而不放在while()循环里。

综上所述，实现一个话题订阅者的步骤大致可分为以下几点：

1. 初始化ROS节点；
2. 订阅需要的话题；
3. 循环等待话题消息，接收到消息后进入回调函数；
4. 在回调函数中完成消息处理。

附相关资料：
(1) Publishers and Subscribers
(2) Writing a Simple Publisher and Subscriber (C++)

3. 配置与编译

3.1 在CMaKeLists.txt中添加编译选项

打开功能包中的CMaKeLists.txt文件。在如下位置的find_package中添加message_generation功能包，以便于（节点）调用它们生成消息。

在如下位置添加相关的.msg文件，确保了CMake在重新配置时知道这些新添加的.msg文件，同时添加 .msg文件在生成消息时的所有依赖项（功能包）。

add_message_files(FILES Person.msg)
generate_messages(DEPENDENCIES std_msgs)

将如下位置中CATLIN_DEPENDS前面的“#”去掉。

在如下位置进行配置，add_executable(Publisher_test src/Publisher_test.cpp)的作用是将src文件夹下的Publisher_test.cpp文件编译成名为Publisher_test的可执行文件。target_link_libraries(Publisher_test ${catkin_LIBRARIES})的作用是将Publisher_test可执行文件与ROS相关的库链接。add_dependencies(Publisher_test ${PROJECT_NAME}_generate_messages_cpp)的作用是将Publisher_test可执行文件与一些动态生成的文件链接。

add_executable(Publisher_test src/Publisher_test.cpp)
target_link_libraries(Publisher_test ${catkin_LIBRARIES})
add_dependencies(Publisher_test ${PROJECT_NAME}_generate_messages_cpp)
	
add_executable(Subscriber_test src/Subscriber_test.cpp)
target_link_libraries(Subscriber_test ${catkin_LIBRARIES})
add_dependencies(Subscriber_test ${PROJECT_NAME}_generate_messages_cpp)

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

打开功能包中的package.xml文件，在如下位置添加功能包依赖。

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

说明：

<build_depend>message_generation</build_depend>

这条语句表示在编译时会依赖一个动态产生message的功能包。

<exec_depend>message_runtime</exec_depend>

这条语句表示在运行时会依赖message_runtime的功能包。

3.3 编译文件

在/ROS_ws文件夹路径下打开一个新的终端，输入以下代码进行编译。

$ catkin_make

编译完成后，输入以下代码运行主节点。

$ roscore

打开一个新的终端，首先配置环境变量，然后输入以下代码运行节点。

$ rosrun test_pkg Publisher_test

打开一个新的终端，首先配置环境变量，然后输入以下代码运行节点。

$ rosrun test_pkg Subscriber_test

若想停止运行，关闭终端，使用快捷键Ctrl+c即可。

4. 话题可视化

打开一个新的终端，输入以下代码。

$ rqt_graph

由此可以得到如下的基于Qt的GUI界面，直观地看到话题通信的发布订阅节点和消息。

5. rostopic指令的使用

常用指令1

$ rostopic list

使用上述指令能够列出所有当前正在订阅和发布的话题，效果如下图。

常用指令2

$ rostopic list -v

使用上述指令能够得到当前正在订阅和发布的话题的详细内容介绍，效果如下图。

常用指令3

更多关于rostopic指令的使用，可以通过以下代码获取。

$ rostopic --help