ROS1学习笔记：话题中的Publisher与Subscriber（ubuntu20.04）

参考B站古月居ROS入门21讲：
发布者Publisher的编程实现

订阅者Subscriber的编程实现
基于VMware Ubuntu 20.04 Noetic版本的环境

一、乌龟例程中的Publisher与Subscriber

我们以小乌龟仿真为例，看一下这个例程中存在那些Publisher(发布者)和 Subscriber(订阅者)。

使用rqt_graph命令查看例程的节点关系图，如下图所示：

当前系统中存在两个节点teleop_turtle和turtlesim。
其中teleop_turtle节点创建了一个Publisher，用于发布键盘控制的速度指令，turtlesim节点创建了一个Subscriber用于订阅速度指令，实现小乌龟在界面上的运动。这里的话题是/turtle1/cmd_vel。

我们再来分析以下这个图：

Publisher，名为Turtle Velocity，即键盘控制海龟的速度指令；
Subscriber，名为/turtlesim，即海龟订阅键盘发送控制速度的指令。

Publisher(Turtle Velocity)，发布Message（即海龟的速度信息，以geometry_msgs::Twist的数据结构，包括线速度和角速度），通过Topic（/turtle1/cmd_vel）总线管道，将数据传输给Subscriber。Subscriber订阅得到的速度信息，来控制海龟发生运动。

“/turtle1/cmd_vel”这个topic是小海龟仿真器节点/turtlesim下自带的topic，可直接使用。

Publisher和Subscriber是ROS系统中最基本、最常用的通信方式。
接下来我们就来通过代码程序来控制小海龟的移动，一起学习如何创建Publisher和Subscriber。

二、创建功能包

在前面已经创建了工作空间了，因此我们直接在src文件夹下创建一个新的功能包learning_topic:

cd ~/catkin_ws/src
catkin_create_pkg learning_topic roscpp rospy std_msgs geometry_msgs turtlesim

三、创建Publisher代码

Publisher的主要作用是针对指定话题发布特定数据类型的消息。

3.1 以C++为例

如何实现一个发布者？

• 初始化ROS节点；
• 向ROS Master注册节点信息，包括发布的话题名和话题中的消息类型；
• 创建消息数据；
• 按照一定的频率循环发布消息。

我们需要在src文件夹(完整路径：/home/ywl/catkin_ws/src/learning_topic/src
)下创建C++的代码文件，并写好代码：

完整代码为：

/***********************************************************************
Copyright 2020 GuYueHome (www.guyuehome.com).
***********************************************************************/

/**
 * 该例程将发布turtle1/cmd_vel话题，消息类型geometry_msgs::Twist
 */
 
#include 
#include 

int main(int argc, char **argv)
{
	// ROS节点初始化
	ros::init(argc, argv, "velocity_publisher");

	// 创建节点句柄
	ros::NodeHandle n;

	// 创建一个Publisher，发布名为/turtle1/cmd_vel的topic，消息类型为geometry_msgs::Twist，队列长度10
	ros::Publisher turtle_vel_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);

	// 设置循环的频率
	ros::Rate loop_rate(10);

	int count = 0;
	while (ros::ok())
	{
	    // 初始化geometry_msgs::Twist类型的消息
		geometry_msgs::Twist vel_msg;
		vel_msg.linear.x = 0.5;
		vel_msg.angular.z = 0.2;

	    // 发布消息
		turtle_vel_pub.publish(vel_msg);
		ROS_INFO("Publsh turtle velocity command[%0.2f m/s, %0.2f rad/s]", 
				vel_msg.linear.x, vel_msg.angular.z);

	    // 按照循环频率延时
	    loop_rate.sleep();
	}

	return 0;
}/***********************************************************************
Copyright 2020 GuYueHome (www.guyuehome.com).
***********************************************************************/

/**
 * 该例程将发布turtle1/cmd_vel话题，消息类型geometry_msgs::Twist
 */
 
#include 
#include 

int main(int argc, char **argv)
{
	// ROS节点初始化
	ros::init(argc, argv, "velocity_publisher");

	// 创建节点句柄
	ros::NodeHandle n;

	// 创建一个Publisher，发布名为/turtle1/cmd_vel的topic，消息类型为geometry_msgs::Twist，队列长度10
	ros::Publisher turtle_vel_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);

	// 设置循环的频率
	ros::Rate loop_rate(10);

	int count = 0;
	while (ros::ok())
	{
	    // 初始化geometry_msgs::Twist类型的消息
		geometry_msgs::Twist vel_msg;
		vel_msg.linear.x = 0.5;
		vel_msg.angular.z = 0.2;

	    // 发布消息
		turtle_vel_pub.publish(vel_msg);
		ROS_INFO("Publsh turtle velocity command[%0.2f m/s, %0.2f rad/s]", 
				vel_msg.linear.x, vel_msg.angular.z);

	    // 按照循环频率延时
	    loop_rate.sleep();
	}

	return 0;
}

3.1.1 配置Publisher代码编译规则

如何配置CMakeLists.txt中的编译规则？

• 设置需要编译的代码和生成的可执行文件
• 设置链接库

将以下代码copy进CMakeLists.txt中:

add_executable(velocity_publisher src/velocity_publisher.cpp)
target_link_libraries(velocity_publisher ${catkin_LIBRARIES})

copy到这个位置：

3.1.2 编译整个工作空间

回到工作空间的根目录执行编译：

cd ~/catkin_ws
catkin_make

3.1.3 配置环境变量

输入以下命令配置环境变量：

source devel/setup.bash

但是这个命令只能在该终端生效，换一个就失效了。有时我们会忘了配置环境变量，因此我们不妨把这个命令放到.bashrc文件下。
这个文件是隐藏文件，我们打开主文件夹，在主文件夹按Ctrl+H显示隐藏文件，如下图所示：

打开这个文件，滑到最下边，将该命令补上，（注意文件路径）：

3.1.4 执行代码

打开多个终端，分别执行以下命令：

roscore

rosrun turtlesim turtlesim_node

rosrun learning_topic velocity_publisher

海龟就按照我们之前代码中设定的线速度和角速度画圆啦！


我们编译好的程序是放在/home/ywl/catkin_ws/devel/lib/learning_topic目录下的：

3.2 以Python为例

3.2.1 查看内置Python版本

我们下载的ubuntu20.04是内置了Python3.8版本的

python3 --version

如图：

3.2.2 配置Publisher代码编译规则

为了区别C++的代码，我们在src的同级目录下创建一个新的文件夹scripts

如何实现一个发布者？

• 初始化ROS节点；
• 向ROS Master注册节点信息，包括发布的话题名和话题中的消息类型；
• 创建消息数据；
• 按照一定的频率循环发布消息。
  我们将Python代码copy进scripts文件夹下：
  
  完整代码为：

#!/usr/bin/env python
# -*- coding: utf-8 -*-

########################################################################
####          Copyright 2020 GuYueHome (www.guyuehome.com).          ###
########################################################################

# 该例程将发布turtle1/cmd_vel话题，消息类型geometry_msgs::Twist

import rospy
from geometry_msgs.msg import Twist

def velocity_publisher():
	# ROS节点初始化
    rospy.init_node('velocity_publisher', anonymous=True)

	# 创建一个Publisher，发布名为/turtle1/cmd_vel的topic，消息类型为geometry_msgs::Twist，队列长度10
    turtle_vel_pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)

	#设置循环的频率
    rate = rospy.Rate(10) 

    while not rospy.is_shutdown():
		# 初始化geometry_msgs::Twist类型的消息
        vel_msg = Twist()
        vel_msg.linear.x = 0.5
        vel_msg.angular.z = 0.2

		# 发布消息
        turtle_vel_pub.publish(vel_msg)
        rospy.loginfo("Publsh turtle velocity command[%0.2f m/s, %0.2f rad/s]", 
				vel_msg.linear.x, vel_msg.angular.z)

		# 按照循环频率延时
        rate.sleep()

if __name__ == '__main__':
    try:
        velocity_publisher()
    except rospy.ROSInterruptException:
        pass

不要忘了给这个py文件打开执行权限：

打开CMakeLists.txt文件，将原来167-170注释的几行代码取消注释，并将默认的my_python_script改成velocity_publisher.py
如图所示：

3.2.3 编译整个工作空间

回到工作空间的根目录，执行编译：

cd ~/catkin_ws
catkin_make

3.2.4 配置环境变量

之间使用C++代码的时候已经加进去了，这里就不用加了。

3.2.5 执行代码

打开多个终端，分别执行以下命令：

roscore

rosrun turtlesim turtlesim_node

rosrun learning_topic velocity_publisher.py

海龟就按照我们之前代码中设定的线速度和角速度画圆啦！

3.2.6 查看当前系统的计算图

新建一个终端，输入以下命令：

rqt_graph

调到Nodes/Topics(active)话题视图：

可以看到有三个节点，publisher节点发布指令(代替键盘控制)通过/turtle1/cmd_vel传速度msg给小海龟仿真器节点/turtlesim，然后使得小海龟移动。

四、创建Subscriber代码

4.1 以C++为例

如何实现一个订阅者？

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

同样地，我们把代码copy进learning_topic的src目录下：

完整代码：

/***********************************************************************
Copyright 2020 GuYueHome (www.guyuehome.com).
***********************************************************************/

/**
 * 该例程将订阅/turtle1/pose话题，消息类型turtlesim::Pose
 */
 
#include 
#include "turtlesim/Pose.h"

// 接收到订阅的消息后，会进入消息回调函数
void poseCallback(const turtlesim::Pose::ConstPtr& msg)
{
    // 将接收到的消息打印出来
    ROS_INFO("Turtle pose: x:%0.6f, y:%0.6f", msg->x, msg->y);
}

int main(int argc, char **argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, "pose_subscriber");

    // 创建节点句柄
    ros::NodeHandle n;

    // 创建一个Subscriber，订阅名为/turtle1/pose的topic，注册回调函数poseCallback
    ros::Subscriber pose_sub = n.subscribe("/turtle1/pose", 10, poseCallback);

    // 循环等待回调函数
    ros::spin();

    return 0;
}

4.1.1 配置Subscriber代码编译规则

如何配置CMakeLists.txt中的编译规则？

• 设置需要编译的代码和生成的可执行文件
• 设置链接库

将以下代码copy进CMakeLists.txt中:

add_executable(pose_subscriber src/pose_subscriber.cpp)
target_link_libraries(pose_subscriber ${catkin_LIBRARIES})

copy进这个位置：

4.1.2 编译整个工作空间

回到工作空间根目录，编译整个工作空间：

cd ~/catkin_ws
catkin_make

4.1.2 配置环境变量

上面已完成环境变量配置，这里可省略。

4.1.4 执行代码

打开多个终端，分别执行以下命令：

roscore

rosrun turtlesim turtlesim_node

rosrun learning_topic pose_subscriber

启动命令后，小海龟的位置就会一直被pose_subscriber监听，由于小海龟没有动，因此小海龟的位置(x,y)不会改变。
我们通过键盘方向键来控制小海龟的移动，新建一个终端，输入以下命令：

rosrun turtlesim turtle_teleop_key

然后我们通过键盘来控制小海龟的移动，这时就会发现坐标发送了变化：

4.2 以Python为例

将源码copy进learning_topic的scripts目录下：

完整代码：

#!/usr/bin/env python
# -*- coding: utf-8 -*-

########################################################################
####          Copyright 2020 GuYueHome (www.guyuehome.com).          ###
########################################################################

# 该例程将订阅/turtle1/pose话题，消息类型turtlesim::Pose

import rospy
from turtlesim.msg import Pose

def poseCallback(msg):
    rospy.loginfo("Turtle pose: x:%0.6f, y:%0.6f", msg.x, msg.y)

def pose_subscriber():
	# ROS节点初始化
    rospy.init_node('pose_subscriber', anonymous=True)

	# 创建一个Subscriber，订阅名为/turtle1/pose的topic，注册回调函数poseCallback
    rospy.Subscriber("/turtle1/pose", Pose, poseCallback)

	# 循环等待回调函数
    rospy.spin()

if __name__ == '__main__':
    pose_subscriber()

别忘了打开执行权限：

4.2.1 配置Subscriber代码编译规则

打开CMakeLists.txt文件，在之前配置发布者的代码编译规则下面加上订阅者的代码编译规则，注意文件名字：

4.2.2 编译整个工作空间

回到工作空间的根目录，执行编译：

cd ~/catkin_ws
catkin_make

4.2.3 配置环境变量

上面已完成环境变量配置，这里可省略。

4.2.4 执行代码

打开多个终端，分别执行以下命令：

roscore

rosrun turtlesim turtlesim_node

rosrun learning_topic pose_subscriber.py

同样地，未移动小海龟时，小海龟的坐标不发生变化。

键盘方向键来控制小海龟的移动，新建一个终端，输入以下命令：

rosrun turtlesim turtle_teleop_key

然后我们通过键盘来控制小海龟的移动，这时就会发现坐标发送了变化：

4.2.5 查看当前系统的计算图

再打开一个终端，输入以下命令：

rqt_graph

可以看到有三个节点，键盘控制器节点/teleop_turtle通过/turtle1/cmd_vel传速度msg给小海龟仿真器节点/turtlesim，/turtlesim通过/turtle1/pose传位置msg给subscriber节点。
/turtle1/cmd_vel和/turtle1/pose两个topic都在turtle1海龟下面，因为小海龟自身会发布速度和位置的msg给这两个topic。