一、话题模型

二、创建learning_topic功能包

cd catkin_ws
catkin_create_pkg learning_topic roscpp rospy std_msgs geometry_msgs turtlesim

并在功能包的src文件夹下创建名为velocity_publisher.cpp 的文件。

cd ~/catkin_ws/learning_topic/src/
touch velocity_publisher.cpp

三、实现一个发布者

velocity_publisher.cpp

/*
* 功能：发布turtle1/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，发布名为/turtle1/cmd_vel的topic，消息类型为geometry_msgs::Twist，队列长度10
	ros::Publisher turtle_vel_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);
	
	//设置循环的频率,单位Hz
	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 velocal command[%0.2f m/s, %0.2f rad/s]",
		vel_msg.linear.x, vel_msg.angular.z);
		//按照循环频率延时
		loop_rate.sleep();
	}	
	return 0;
}
 

该publisher实现的是发布turtle1/cmd_vel话题，小海龟作为订阅者接收消息后进行运动，相当于控制小海龟运动。消息类型为Twist，是ROS已经定义好的，所以这里直接包含Twist相关的头文件<geometry_mags/Twist>。

接下来是主体部分的分析：

1. 初始化ROS节点

//ROS节点初始化
ros::init(argc, argv, "velocity_publisher");

函数如其名，init(初始化)，可在 ros/init.h 头文件中找到其声明如下：

ROSCPP_DECL void init(int &argc, char **argv, const std::string& name, uint32_t options = 0);

/**
 * \brief alternate ROS initialization function.
 *
 * \param remappings A map<string, string> where each one constitutes a name remapping, or one of the special remappings like __name, __master, __ns, etc.
 * \param name Name of this node.  The name must be a base name, ie. it cannot contain namespaces.
 * \param options [optional] Options to start the node with (a set of bit flags from \ref ros::init_options)
 * \throws InvalidNodeNameException If the name passed in is not a valid "base" name
 */

包含三个参数，前两个参数是命令行或launch文件（见第19讲）输入的参数，可用于完成命名重映射等功能；第三个参数定义了Publisher节点的名称，该名称在运行的ROS中必须是独一无二的，不允许同时存在相同名称的两个节点。

2. 注册节点信息

向 ROS Master 注册节点信息，包括发布的话题名和话题中的消息类型。

//创建节点句柄
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);

这里节点句柄(NodeHandle)，顾名思义是节点门把手，门后的世界是什么样的需要你来规定，在这段程序中规定门后面通往Pulisher的一个对象。(和《黑客帝国》里的门很像，需要特定的key才能决定门通往哪里)

先来看 ros/node_handle.h 中该函数模板的声明：

 template <class M>
    Publisher advertise(const std::string& topic, uint32_t queue_size, bool latch = false)
    {
      AdvertiseOptions ops;
      ops.template init<M>(topic, queue_size);
      ops.latch = latch;
      return advertise(ops);
    }

该函数模板返回一个Publisher类的对象，有三个参数。

• 第一个参数表示话题名称
• 第二个表示消息发布队列的大小
• 第三个参数latch中文意思为门闩，表示闭锁，与多进程/线程有关，这里不作讨论，默认值为false。

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

所以这里节点句柄指定以geometry_msgs::Twist类型构造advertise()函数，该函数初始化一个Publisher类的对象turtle_vel_pub，并以参数形式规定该对象发布名为/turtle1/cmd_vel的话题，消息发布队列大小为10，且消息类型为geometry_msgs::Twist。

3. 创建消息数据

在进入循环之前，设置循环频率，否则程序将以发布消息的最大频率发布消息：

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

这里设置的是10Hz。当调用Rate::sleep()时，ROS节点会根据此处设置的频率休眠相应的时间，以保证循环维持一致的时间周期。

接下来进入循环：

while(ros::ok())
{
	//loop body
}

在节点未发生异常的情况下将一直在循环中运行，一旦发生异常，ros::ok()就会返回false，跳出循环。
这里的异常情况主要包括：

• 收到SIGINT信号(Ctrl+C)；
• 被另外一个相同名称的节点踢掉线；
• 节点调用了关闭函数ros::shutdown()；
• 所有ros::Nodehandle 句柄被销毁。

循环体分析：

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

在 geometry_msg/Twist.h 头文件中找到如下Twist结构体声明：

namespace geometry_msgs
{
template <class ContainerAllocator>
struct Twist_
{
  typedef Twist_<ContainerAllocator> Type;

  Twist_()
    : linear()
    , angular()  {
    }
  Twist_(const ContainerAllocator& _alloc)
    : linear(_alloc)
    , angular(_alloc)  {
  (void)_alloc;
    }



   typedef  ::geometry_msgs::Vector3_<ContainerAllocator>  _linear_type;
  _linear_type linear;

   typedef  ::geometry_msgs::Vector3_<ContainerAllocator>  _angular_type;
  _angular_type angular;

在 geometry_msgs/Vector3.h 中有：

namespace geometry_msgs
{
template <class ContainerAllocator>
struct Vector3_
{
  typedef Vector3_<ContainerAllocator> Type;

  Vector3_()
    : x(0.0)
    , y(0.0)
    , z(0.0)  {
    }
  Vector3_(const ContainerAllocator& _alloc)
    : x(0.0)
    , y(0.0)
    , z(0.0)  {
  (void)_alloc;
    }



   typedef double _x_type;
  _x_type x;

   typedef double _y_type;
  _y_type y;

   typedef double _z_type;
  _z_type z;

即 x，y，z 是linear和angular的成员变量，linear和angular是Twist结构体的成员变量。

发布封装完毕的消息：

turtle_vel_pub.publish(vel_msg);

Master会查找订阅该话题的节点，并帮助两个节点建立连接，完成消息的传输。

ROS_INFO("Publish turtle velocal command[%0.2f m/s, %0.2f rad/s]",
		vel_msg.linear.x, vel_msg.angular.z);

这里 ROS_INFO 类似于C/C++中的printf/cout 函数，用来打印日志信息。这里将发布的数据在本地打印，以确保发出的数据符合要求。

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

至此Publisher一个周期的工作已完成，需要让节点休眠一段时间，然后根据之前设置的循环频率10Hz按时唤醒，即休眠100ms后Publisher又继续发布消息。

四、配置CMakeLists.txt中的编译规则

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

//添加生成可执行文件 add_executable(目标可执行文件名 源文件路径)
add_executable(velocity_publisher src/velocity_publisher.cpp)
//将该生成的可执行文件加入链接库中
target_link_libraries(velocity_publisher ${catkin_LIBRARIES})

注意上述两行位置不可交换，只有生成可执行文件后才能加入链接库。

打开该功能包下的CMakeLists.txt文件并添加：

五、编译并运行发布者

1. 编译

CMakeLists.txt修改完成后，需要对工作空间进行编译，注意回到工作空间路径下进行编译。

cd ~/catkin_ws
catkin_make

2. 运行

source devel/setup.bash

在执行之前设置ROS环境变量，或者将这行命令直接放入home下的 .bashrc 中。

终端1：

roscore

终端2：打开海龟仿真器

rosrun turtlesim turtlesim_node

终端3：运行生成的可执行文件

rosrun learning_topic velocity_publisher

可以观察到海龟在按圆形轨迹运动

同时终端输出显示海龟当前速度和角速度：

参考资料
[1]胡春旭.ROS机器人开发实践[M].北京:机械工业出版社,2018:506.