ROS --- 编写消息发布器和订阅器

编写发布器节点

  现在，我们想通过编写一个发布器来使小海龟做圆周运动，发布器为小海龟提供速度信息。
  此前，我们知道了节点之间进行通讯时，传递的消息是具有一定的名称和消息类型的，所以要想保证 /turtlesim接收到正确的信息，发布器在发布消息时，一定要发布/turtlesim所对应的消息名称 和 消息类型。
  首先，打开并运行海龟仿真器，调用rostopic list，获知速度“话题名”为 /turtle1/cmd_vel，调用rostopic type /turtle1/cmd_vel ，返回的“消息类型”为geometry_msgs/Twist。知道这些之后，往后的发布器的编写会有用的。

建立一个功能包（存放发布者）

$ cd ~/catkin_ws/src      % 进入到 /src 目录

% 接下来创建一个learning_topic的文件夹；
%其中依赖的功能包，关注geometry_msgs和turtlesim，geometry_msgs是消息类型相关的功能包，turtlesim自然不用多说。

$ catkin_creat_pkg learning_topic roscpp rospy std_msgs geometry_msgs turtlrsim  

  这时候，catkin_creat_pkg 就帮我们创建了learning_topic 下的include、src、CMakeLists.txt以及package.xml。我之后会对其进行一些修改，使其能正确编译。

编写发布器（C++）

  发布器的完整源码如下，之后稍作解释：

  /**
    * 该例程将发布 /turtle/cmd_vel 话题，消息类型为 geometry_msgs::Twist
    */
 
 # include <ros/ros.h>
 # include <geometry_msgs/Twist.h>
  
 int main(int argc, char **argv)
 {
    	 // ROS节点初始化
    	 ros::init(argc, argv, "velocity_publisher");
 
	     // 创建节点句柄
 	    ros::NodeHandle n;

	    // 创建一个publisher，发布名为 /turtle/cmd_vel 的 topic，消息类型为 geom    etry_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("Publish turtle velocity command[%.2f m/s, %.2f rad/s]"    , vel_msg.linear.x, vel_msg.angular.z);

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

    	return 0;
}publish()

  下面结合代码，梳理一下编写一个发布器的步骤：

初始化 ROS 节点

  1. 头文件：
  首先，程序要包含的头文件： <ros/ros.h>和 <geometry_msgs/Twist.h>。
   <ros/ros.h>：是ROS的一个API ( Application Programming Interface，应用程序接口 )头文件，其引用了 ROS 系统中绝大部分常用的头文件。
  <geometry_msgs/Twist.h>：是对线速度、角速度定义消息类型的头文件，这个是由ROS在geometry_msgs库已经定义好的。

  2. main函数：

int main(int argc, char *argv[])

  之前见的main函数一般都是无参数的。这种指定参数的main函数写法实际上是最严格、系统最纯正的写法。
  特殊之处是，argc和argv只有在命令行编译程序的时候才有效。
  argc为参数数量，为int型，为系统生成的，argv为参数数组，用户只需要输入具体的参数即可，无需输入参数数目，系统会根据输入的参数自动补充参数数目。
  更为详细的阐释，可以参考以下大佬的博文：
   含参main函数（外部链接 1）
   含参main函数（外部链接 2）

 ros::init(argc, argv, "velocity_publisher")

  这条语句主要是指定该节点的名称，一定注意，节点名称必须是唯一的，ROS不允许再有相同名称的节点存在，一般情况下，argc和argv参数不用进行设置。
  3. 创建句柄：

ros::NodeHandle n;

  这里为这个进程的节点创建一个句柄。句柄，可以理解为节点与ROS系统访问通讯的通道。可以这样形象地理解，Handle就是一个“门把手”，ROS系统通过这个“门把手”就可以执行对应的这个“屋子”里所有的功能。
  这里创建的句柄 n，后续会通过其来创建publisher对象。

向ROS Master注册节点信息

ros::Publisher turtle_vel_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);

   此前，我们已经知道了速度的“话题名”为 /turtle1/cmd_vel，“消息类型”为geometry_msgs/Twist。
   以上语句创建了一个publisher，将在**/turtle/cmd_vel**（话题名）上发布消息类型为 geometry_msgs::Twist 的 topic，这样Master就会告诉所有订阅了/turtle1/cmd_vel/ 的节点，这里有一个发布者也将发布数据了。第二个参数为发布序列的队列长度，由于话题通讯属于异步通讯，为了解决发布/接受不同步，这里相当于一个缓冲区，这个序列中适中存放最近更新的（队列长度）个数据。
   NodeHandle::advertise() 返回一个 ros::Publisher 的对象，他有两个作用：1）它有一个publish() 成员函数能够让你在topic上发布消息；2）它会检查消息类型是否对应，如果消息类型不对，他会拒绝发布。

创建消息数据

 while(ros::ok())

  这里的 ros::ok() 返回布尔值，roscpp会默认生成一个SIGINT句柄，当进行键盘操作Ctrl-C 时，这个SIGINT会被触发，使得 ros:ok() 返回 False；同时，ros::shutdown() 被程序的另一部分调用时也会使其返回False。

// 初始化geometry_msgs::Twist类型的消息                             
geometry_msgs::Twist vel_msg;
vel_msg.linear.x = 0.5;
vel_msg.angular.z = 0.2;

  这里创建了一个geometry_msgs::Twist类型的变量 vel_msg，之后按照该消息类型的结构对变量进行初始化，即编写消息内容。

发布消息

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

  此前我们创建了一个 ros::Publisher 对象，名为 turtle_vel_pub，该对象下的成员函数 publish() 使上一步创建的 “消息内容” 发布出去。
  ROS_INFO语句类似于 “printf / cout” 函数，用来在终端打印一些显示信息。

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

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

  ros::Rate 对象可以允许指定自循环的频率，括号中为循环频率。它会追踪记录自上一次调用 Rate::sleep() 后时间的流逝，并休眠直到一个频率周期的时间。所以需要在消息发布循环结构中，写上一句 loop_rate.sleep()，来使程序执行完一次发布之后调用休眠函数，loop_rate对象追踪休眠时间，休眠时间满，继续执行后续程序。
  这就是搭建一个发布器的大致步骤了，其他发布器的搭建大同小异，一定注意消息类型和消息名称的对应，按照消息类型的结构进行编写。

编译发布器

   编写好发布器之后，需要在对应功能包中的编译链接文件中，添加源文件的相应库连接。打开发布器所在功能包文件夹下的CMAkeLists.txt 文件，在其中Build段下添加相关链接。

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

// Declare a C++ executable
// With catkin_make all packages are built within a single CMake context
// The recommended prefix ensures that target names across packages don't collide
// add_executable(${PROJECT_NAME}_node src/learning_topic_node.cpp)

// Specify libraries to link a library or executable target against
// target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES} )

   主要在想位置替换相应的节点名称和节点源文件，修改之后保存并关闭。在工作空间根目录下，执行“catkin_make”命令，进行编译，编译成功之后，在工作空间下 /devel 文件夹相应的功能包名称下可以找到相应的源文件编译产生的可执行文件。

编写订阅器节点

  编写的订阅器源码同样放在 /src 文件夹下，下面是在“海龟仿真器”的基础上，添加一个节点，订阅海龟的位姿信息，再将其显示出来。（源码来自于“古月居”）

编写订阅器（C++）

  源码如下：

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

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

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

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

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

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

   具体步骤如下，相对发布器的编写，订阅器步骤相似

初始化 ROS 节点

   首先还是要先初始化一个节点，并对其进行命名。

ros::init(argc, argv, "pose_subscriber");

订阅需要的话题

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

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

   告诉 master 我们要订阅 /turtle1/pose 话题上的消息。当有消息发布到这个话题时，ROS 就会调用 poseCallback() 函数。第二个参数是队列大小，以防我们处理消息的速度不够快，来适应异步通讯方式，当缓存达到 10 条消息后，再有新的消息到来就将开始丢弃先前接收的消息，总是使用队列中最新的数据。
   NodeHandle::subscribe() 返回 ros::Subscriber 对象。当这个对象销毁时，它将自动退订 /turtle1/pose 话题的消息。

循环等待，回调函数

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

   ros::spin() 进入自循环，有消息进来，则进入回调函数（类似于嵌入式中的中断程序），可以尽可能快的调用消息回调函数。如果没有消息到达，它不会占用很多 CPU，所以不用担心。一旦 ros::ok() 返回 false，ros::spin() 就会立刻跳出自循环。这有可能是 ros::shutdown() 被调用，或者是用户按下了 Ctrl-C，使得 master 告诉节点要终止运行。也有可能是节点被人为关闭的。

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

   回调函数，当接收到 /turtle1/pose 话题的消息之后，就会被调用。消息一般用指针或者引用传递，不用复制数据，能够保证数据传递的高效。在回调函数中，进行与消息相关的操作。

编译订阅器

   编写好发布器之后，需要在对应功能包中的编译链接文件中，添加源文件的相应库连接。打开发布器所在功能包文件夹下的CMAkeLists.txt 文件，在其中Build段下添加相关链接。

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

// Declare a C++ executable
// With catkin_make all packages are built within a single CMake context
// The recommended prefix ensures that target names across packages don't collide
// add_executable(${PROJECT_NAME}_node src/learning_topic_node.cpp)

// Specify libraries to link a library or executable target against
// target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES} )

   主要在想位置替换相应的节点名称和节点源文件，修改之后保存并关闭。在工作空间根目录下，执行“catkin_make”命令，进行编译，编译成功之后，在工作空间下 /devel 文件夹相应的功能包名称下可以找到相应的源文件编译产生的可执行文件。

发布器和订阅器编写实例

发布器

#include "ros/ros.h"
#include "std_msg/String.h"

#include <sstream>

int main(int argc, char** argv)
{
	// 初始化节点；
	ros::init(argc, argv, "talker");
	// 初始化节点句柄；
	ros::NodeHandle n;
	// 使用句柄对象n下的类模板 NodeHandle::advertise()，创建一个ros::Publisher 对象。参数一：节点名称；参数二：队列长度。
	ros::Publisher chatter_pub = n.advertise<std_msg::String>("chatter",1000);
	// 创建ros::Rate对象 loop_rate， 参数为设置的频率大小。
	ros::Rate loop_rate(10);

	int count = 0;
	while (ros::ok())
	{
		// 创建ROS标准消息字符串对象 msg；
		std_msgs::String msg;
		// 使用C++中的标准输入输出流对象对msg.data进行赋值；
		std::stringstream ss;
		ss << "hello world " << count;
		msg.data = ss.str();
		
		// 打印消息；
		ROS_INFO("%s", msg.data.c_str());
		// 发布消息；
		chatter_pub.publish(mag);
		// 以之前设定的发布频率进行休眠循环；
		loop_rate.sleep();
		++count;
	}
		return 0;
}

发布器

#include "ros/ros.h"
#include "std_msgs/String.h"

void chatter