ROS入门之Topic和Server编程

Topic和Server是ROS中非常重要的两种通信机制，下面分别对其进行介绍。

Topic

Publisher和Subscriber

Topic的结构如图所示：

Publisher节点按照规定格式发布消息到Topic中，Subscriber通过订阅Topic来获取数据。

下面是使用Publisher来控制turtle速度的例子：

#include
#include

int main(int argc, char **argv)
{
    # 初始化Node
    ros::init(argc, argv, "velocity_publisher");
    # 创建Node句柄，对当前Node进行操作
    ros::NodeHandle n;
    # 定义一个Publisher，对“/turtle1/cmd_vel“进行订阅
    # Publisher由NodeHandle.advertise函数进行创建，模版中确定该
    # Publisher定义的topic中的消息结构，函数参数为Topic名称和消息
    # 队列的长度
    ros::Publisher turtle_vel_pub = n.advertise("/turtle1/cmd_vel", 10);
    # 定义循环的频率
    ros::Rate loop_rate(10);
    # 配合loop_rate控制程序执行的频率
    while (ros::ok())
    {
        # 定义消息类型实例
        geometry_msgs::Twist vel_msg;
        # 线速度
        vel_msg.linear.x=0.5;
        # 角速度
        vel_msg.angular.z = 0.2;
        # 向topic中发布消息
        turtle_vel_pub.publish(vel_msg);
        # 向控制台输出log
        ROS_INFO("publish turtle velocity command [%0.2f m/s, %0.2f rad/s]", vel_msg.linear.x, vel_msg.angular.z);
        # 控制循环频率
        loop_rate.sleep();
    }
    return 0;
}

实现一个Publisher主要流程为：

1. 初始化ROS节点并生成一个句柄
2. 实例化Publisher，定义需要publish的消息类型，Topic，buffer size等信息
3. 创建消息
4. 按照一定频率发布消息

在控制turtlesim的运动之后，我们还可以实时接收turtle的位置信息，具体代码如下：

#include
#include "turtlesim/Pose.h"

# 回调函数
void poseCallback(const turtlesim::Pose::ConstPtr& msg)
{
    # 向控制台输出log信息
    ROS_INFO("turtle pose: x:%0.6f, y:%0.6f", msg->x, msg->y);
}

int main(int argc, char **argv)
{
    # 初始化节点
    ros::init(argc, argv, "pose_subscriber");
    # 创建句柄
    ros::NodeHandle n;
    # 定义Subscriber对“/turtle1/pose”进行subscribe
    # n.subscribe这里定义的是一个回调函数的形式，并不会立即执行
    # 其参数为 (Topic name, buffer size, callback func)
    ros:: Subscriber pos_sub = n.subscribe("/turtle1/pose", 10, poseCallback);
    # 循环等待接收Topic发来的数据，调用回调函数
    ros::spin();
    return 0;
}

实现一个订阅者的主要流程为：

1. 初始化ROS节点并创建句柄
2. Subscribe一个Topic，并定义回调函数
3. 循环等待消息，并在回调函数中进行处理

上述代码都需要遵循cmake的编译规则，在相应的CMakeList.txt中进行配置。以Subscriber为例，配置如下：

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

修改好CMake配置之后，再进入catkin_ws文件夹执行catkin_make命令进行编译。编译完成后，使用rosrun命令运行Node，如图所示：

同样，Publisher和Subscriber也可以用python代码实现，ROS中默认使用的是python2.7，一般在package下新建一个script文件夹存放python文件。Publisher和Subscriber的python代码如下：

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# velocity_publisher.py
import rospy
from geometry_msgs.msg import Twist

def velocity_publisher():
    rospy.init_node("velocity_publisher", anonymous=True)

    turtle_vel_pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
    rate = rospy.Rate(10)

    while not rospy.is_shutdown():
        vel_msg = Twist()
        vel_msg.linear.x = 0.5
        vel_msg.angular.z = 0.5
        turtle_vel_pub.publish(vel_msg)
        rospy.loginfo("Publish 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

Subscriber代码如下：

#!/usr/bin/env python
# pose_subscriber.py
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():
    rospy.init_node('pose_subscriber', anonymous=True)
    rospy.Subscriber('/turtle1/pose', Pose, poseCallback)
    rospy.spin()

if __name__ == "__main__":
    pose_subscriber()

python代码无需编译，因此也不需要对CMakelist.txt进行修改，但是需要给python文件添加执行权限，如：

chmod +x /home/mastercai/catkin_ws/src/learning_topic/script/velocity_publisher.py 

然后直接执行rosrun命令即可：

rosrun learning_topic velocity_publisher.py

自定义massage

除了使用ROS中预定义好的massage，ROS还可以自定消息类型，下面以Person信息来举例说明。

ROS中的自定义消息通过.msg文件定义，一般都放在package目录下的msg文件夹。在msg文件夹下新建Person.msg文件：

string name
uint8 sex
uint8 age

uint8 unknown = 0
uint8 male = 1
uint8 female = 2