ROS学习笔记（二）-----发布者publisher的编程实现、订阅者Subscriber的编程实现、话题消息的定义和使用

   本系列文章是与大家分享一下，我在学习ROS过程中所做的笔记，目前主要的学习资料是古月老师（胡春旭老师）的 ROS入门21讲，以及其编著的《ROS机器人开发实践》，当然其中也加入了我自己的理解和其他的相关资料，本系列的各篇文章将处于不断的更新完善中，本篇文章是本系列第二篇文章

---

五、发布者publisher的编程实现

   1、ROS Master 是节点管理器，在下图的模型中，订阅者是海龟仿真器turtlesim，发布者是本部分我们需要通过编程实现的Turtle Velocity，他发布的信息Message得数据结构是我们之前接触过的Twist，包含线速度和角速度，通过Topic总线管道把数据传输给Subscriber，Subscriber通过订阅得到数据，并控制海龟运动

   2、我们先用catkin_create_pkg命令来创建一个功能包（要在工作空间下的src文件夹内创建），取名为learning_topic,需要添加的依赖如下图所示：

   3、创建一个发布者的流程

   4、在我们创建的learning_topic 功能包的src文件夹下编写我们要实现的发布者的代码（下面是c++代码）

**
 * 该例程将发布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;
}
~                                                                                               
~                     

   （1）在程序的开始我们要编写我们应该包含的头文件，在这个例程中我们包含了#include 和#include 这两个头文件，我们用到的很多的ros的函数的定义都是在#include 里面定义的，在geometry_msgs库里面已经定义好了我们需要的Twist的数据类型，所以我们需要包含头文件#include

   （2）在main函数里面，我们首先要通过ros::init 完成对节点的初始化，argc,和argv是main函数的输入参数，将该节点取名为 velocity_publisher （注意，在ROS系统中节点名不能重复）

   （3）接下来 ros::NodeHandle n是创建节点句柄，节点句柄是用来管理ROS相关的API资源的，比如创建发布者API的调用，都是用句柄来调用的

   （4）接下来就是创建一个名为turtle_vel_pub 的Publisher，发布名为/turtle1/cmd_vel的topic，消息类型为geometry_msgs::Twist，>队列长度10， （当我们发送能力不足时，会将待发送的内容放到队列里去，若队列满了，就删除先进队的数据，丢帧）， 注意发布者定义的话题名需要与订阅者订阅话题名相同

   （5） 接下来用设置loop_rate设置下面的while循环的频率

   （6）在while循环里面，我们要封装数据，并且发布出去，然后延时来满足我们的频率进入下次循环。首先我们要设置要发布的消息的内容，我们要创建Twist类的对象，取名为vel_msg，接下来两行语句就是设置我们想要的线速度和角速度，很显然上面程序中设定的是一个圆的轨迹，接下来就是用我们刚才创建的发布者，去发布我们发布的数据对象vel_msg，接下来的语句是用来日志输出的，去告诉我们的客户端，信息已经发布出去了，发不了什么样的信息，接下来就是我们之前设定的延时了，10Hz，也就是1/10秒的时间

   5、接下来就是去编译和运行它，编译时我们需要去配置之前生成的CMakeLists.txt文件（这个文件位于我们之前创建的功能包下）中的编译规则，在该文件如下所示的位置（也就是build段的最后）添加两行代码，第一行语句是用来描述把哪一个cpp文件编译成可执行文件的，第二行语句是把可执行文件去跟ROS的一些库去做链接的

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

   6、接下来就是进行编译了，同样编译时需要先进入工作空间的根目录下，使用catkin_make命令来编译，编译完成后接下来就是利用命令source devel/setup.bash去设置环境变量，为了不在每次运行时都去设置环境变量，可以将该语句添加到主文件下的.bashrc文件中，该文件是隐藏文件，需要按ctrl+h才能显示出来，打开后，在最后添加source devel/setup.bash ，然后需要修改一下路径改成该工作空间下的路径，比如我的是source /home/jzx/catkin_ws/devel/ setup.bash 重新启动终端生效，有了这一步操作后，就不用每次都运行source devel/setup.bash了，而且可以在任意路径下运行rosrun命令

   7、接下来我们运行roscore，打开节点管理器，运行rosrun turtlesim turtlesim_node 海龟仿真器，然后执行我们自己编写的功能包learning_topic里的节点velocity_publisher去控制海龟运行

   8、为了与c++代码区分开，我们在功能包文件夹下新建一个名为scripts的文件夹存放Python文件，对于Python文件，我们需要右键点击属性，权限，将允许文件作为程序文件执行的对勾打上

   9、用Python实现上述功能的代码

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 该例程将发布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

  10、python不用编译 直接输入rosrun learning_topic velocity_publisher.py 命令就可以执行上面的文件了（但是我的显示错误ImportError: No module named yaml，而且确认我的ubuntu里面有yaml，暂时还没找到解决办法）

---

六、订阅者Subscriber的编程实现

  1、实现订阅者的流程：

   2、c++的代码如下：

/**

 * 该例程将订阅/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;
}

   （1）、以上代码要完成的任务是订阅/turtle1/pose话题，消息类型turtlesim::Po se

   （2）、首先要包含ros的头文件和消息类型的头文件，即 #include 和#include “turtlesim/Pose.h”

   （3）、我们先看一下main函数，在main函数一开始我们就要去初始化节点，所有ros进到节点之前就是先初始化节点

   （4）、 接下来的 ros::NodeHandle n 是创建句柄去管理节点资源

   （5）、 接下来是创建我们的Subscriber，利用ros里面的Subscriber 类，创建这个类的对象取名为pose_sub ，他的初始化信息包含去定义那个话题，在本例中就是订阅/turtle1/pose这个话题，第二个参数是订阅的话题的队列长度，第三个参数是回调函数名

   （6）、我们的订阅者并不知道发布者什么时候有消息进来，一旦有信息进来，订阅者就立马调用回调函数去处理，比较像我们在嵌入式里面学习的中断这样的机制，一旦有Subscriber订阅的数据进来，就会立马跳到回调函数里面去做处理

   （7）、接下来的语句 ros::spin()，spin()是ros里面的一个循环等待的死循环，在spin()里面会不断的查看队列是否有消息进来，如果有消息进来就会调用回调函数进行处理，没有消息进来就死循环，所以在正常情况下不会进入后面的 return 0;语句

   （8）、接下来我们来看一下这个回调函数的内容，这个函数的参数是针对消息的一个指针，const turtlesim::Pose::ConstPtr& msg 这是在ros里面固定格式的一个调用，turtlesim::Pose 对应于我们主函数里面话题内容/turtle1/pose是一样的，后面的ConstPtr是一个常指针，msg以一个常指针的形式指向turtlesim::Pose所有的姿态信息的数据内容

   （9）、（当消息自动生成到C++代码时，定义了几种类型。其中一个是::Ptr，它被类型化为boost::shared_ptr，另一个是::ConstPtr，它是boost::shared_ptr。通过将const指针传递到回调）

   （10）、下面我们通过对这个指针里面的数据调用得到海龟当前的位置信息：ROS_INFO(“Turtle pose: x:%0.6f, y:%0.6f”, msg->x, msg->y)通过ROS_INFO将海龟的位置信息发布到终端，我们就可以看到海龟的实时位置信息了

   3、接下来就是配置订阅者代码编译规则，我们将如下的编译规则内容拷贝到CMakeLists.txt中，添加在之前的编写发布者时添加的位置后面就行

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

   4、然后在工作空间的根目录下，使用catkin_make进行编译，接下来就是 roscore rosrun turtlesim turtlesim_node rosrun learning_topic velocity _publisher 来运行了（如果不输入rosrun learning_topic velocity _publisher，也就是不运行发布者的话，直接运行订阅者，我们得到的数据是不动的，或者可以通过键盘控制海龟移动也行）（由于之前在编写发布者的时候将环境变量添加到了.bashrc文件中，所以在此处就不用设置环境变量了）

$ cd ~/catkin_ws
$ catkin_make
$ source devel/setup.bash
$ roscore
$ rosrun turtlesim turtlesim_node
$ rosrun learning_topic velocity_publisher 

   5、Python代码如下：

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 该例程将订阅/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()

七、话题消息的定义和使用

   1、话题模型

   2、当ros中已经定义好的消息无法满足我们的需求时，我们就需要自己去定义我们需要的消息，本部分内容我们要完成对名为person的这样一个信息的传送，包括年龄、性别、等信息，我们需要把我们定义的这些消息放在一个.msg文件中，取名为Person

   3、➢ 定义msg文件：首先我们先来创建Person.msg这样一个文件，我们打开之前我们创建的功能包learning_topic ,在该文件夹下创建一个新的文件夹取名为msg，在msg文件夹下新建Person.msg文件（可以通过命令 touch Person.msg来完成创建）

   然后我们打开这个文件，把以下内容添加到该文件中，ros在编译的时候就会根据我们c++或者Python的一些定义，去编译成c++或者Python的程序，到这里我们完成了数据接口的定义

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

   4、接下来就是根据数据接口的定义去设置一些编译的规则，我们先在package.xml中添加如下的功能包依赖，我们添加的是一个动态生成程序的一个功能包依赖，添加在如下图所示位置，第一个是编译依赖，动态产生message的功能包message_generation，第二个是运行依赖，依赖message_runtime这样一个功能包

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

   5、然后在CMakeLists.txt中添加编译选项，打开该文件

   （1） 在该文件的find_package中添加功能包message_generation 如下所示：

   （2）添加如下的把msg文件配置成不同的程序文件的配置项，添加到如下图所示位置，第一个是add_message_files 他会把Person.msg作为我们定义的消息接口，在编译的时候就会发现这个消息接口，并针对他进行编译，第二个generate_messages是我们在编译Person.msg这个文件时需要依赖ros那些已有的库或者包

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

   （3）在catkin_package里面去创建运行的依赖，我们需要将如下所示的该行代码的注释取消掉，然后在他后面添加message_runtime

   5、然后我们回到工作空间的根目录下进行编译（命令：catkin_make）, 编译成功后我们可以看一下刚才的.msg文件所生成的一些代码文件，Person.h就是根据刚才的.msg文件编译生成的c++的头文件

   6、在如下目录下编写好发布者和订阅者的程序

   7、发布者的c++代码如下：

/**

 * 该例程将发布/person_info话题，自定义消息类型learning_topic::Person

 */
#include 
#include "learning_topic/Person.h"

int main(int argc, char **argv)
{
     
   // ROS节点初始化
   ros::init(argc, argv, "person_publisher");
   // 创建节点句柄
   ros::NodeHandle n;
  // 创建一个Publisher，发布名为/person_info的topic，消息类型为learning_topic::Person，队列长度10
    ros::Publisher person_info_pub = n.advertise<learning_topic::Person>("/person_info", 10);
    // 设置循环的频率
    ros::Rate loop_rate(1);
    int count = 0;
    while (ros::ok())
    {
     
       // 初始化learning_topic::Person类型的消息
    	learning_topic::Person person_msg;
		person_msg.name = "Tom";
		person_msg.age  = 18;
		person_msg.sex  = learning_topic::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();
    }
    return 0;
}

   （1）可以发现他调用的第二个头文件就是我们之前生成的Person.h文件，创建的名为person_info_pub的发布者，去发布我们定义的learning_topic::Person这样的一个消息接口的（这个消息接口就是通过#include "learning_topic /Person.h"去调用的）他往person_info这样的话题去发，队列长度是10

   （2）在while循环里去创建Person类的对象 person_msg ,在对其成员性别进行定义的时候，我们利用这样的宏定义learning_topic::Person::male去调用我们之前在msg中定义的宏

   （3）其他的地方跟之前介绍的发布者的创建相同，在这里就不介绍了

   8、订阅者的c++代码如下：

/**

 * 该例程将订阅/person_info话题，自定义消息类型learning_topic::Person

 */
#include 
#include "learning_topic/Person.h"
// 接收到订阅的消息后，会进入消息回调函数
void personInfoCallback(const learning_topic::Person::ConstPtr& msg)
{
     
    // 将接收到的消息打印出来
    ROS_INFO("Subcribe 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, "person_subscriber");
    // 创建节点句柄
   ros::NodeHandle n;
   // 创建一个Subscriber，订阅名为/person_info的topic，注册回调函数personInfoCallback
    ros::Subscriber person_info_sub = n.subscribe("/person_info", 10, personInfoCallback);
   // 循环等待回调函数
    ros::spin();
    return 0;
}

   9、接下来就要去配置代码的编译规则了，我们把以下的代码复制到CMakeLists.txt中，位置如下所示，与之前添加的相比多了add_dependencies 这个是用来将可执行文件和我们动态生成的程序去产生一些依赖关系，跟自定义的消息进行连接的话必须要添加这样一句话

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

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

   10、现在我们再返回工作空间的根目录下进行编译，编译完以后就可以运行查看，以上程序所实现的效果了，首先我们先打开节点管理器，然后再运行发布者和订阅者（没有顺序，先打开那个都行），这个时候就可以发现两者进行了数据通信了，在此时关闭节点管理器不影响他们之间的数据传输，因为节点管理器只是帮助他们建立起连接，不参与他们的数据传输，一旦连接建立完成，它就不发挥作用了

---