ROS学习记录（三）——Publisher和Subscriber编程实现

ROS学习记录（一）——基本概念
ROS学习记录（二）——创建功能包
ROS学习记录（三）——Publisher和Subscriber编程实现
ROS学习记录（四）——话题消息的定义与使用
ROS学习记录（五）——Client和Server编程实现

平台：Ubuntu18.04，ROS-melodic
实验目的：学习发布者Publisher和订阅者Subscriber的编程实现。

Publisher可以创建新话题，并可以向其发消息。Subscriber是订阅话题的。
在定义Publisher的时候，需要明确消息数据类型，而定义Subscriber时不用
Subscriber涉及到回调函数，Publisher没有
ros::spin()、ros::spinOnce()用在接受数据的情况下，如Subscriber、Server中

实验1：发布者Publisher编程

实验1(C语言版本)

设想编写Publisher，控制海龟进行匀速圆周运动。

步骤：

1. 依次执行mkdir ~/catkin_ws，cd catkin_ws，mkdir src，catkin_init_workspace进行初始化工作空间，catkin_create_pkg test_pkg roscpp rospy std_msgs geometry_msgs创建功能包，cd ~/catkin_ws/src/test_pkg/src进入功能包存放代码的地方，touch turtle_test.cpp
2. 打开turtle_test.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"); // 节点名字为“velocity_publisher”

	// 创建节点句柄
	ros::NodeHandle n;  // 句柄是用来管理节点资源的，在后续调用操作中

	// 创建一个Publisher，发布名为/turtle1/cmd_vel的话题，消息类型为geometry_msgs::Twist，队列长度10
	ros::Publisher turtle_vel_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);
	// 上述为交代了向哪个话题发布数据，即“/turtle1/cmd_vel”。队列大小就相当于发布的数据缓冲区的大小

	// 设置循环的频率
	ros::Rate loop_rate(10);
	// 下方有一个while循环，用来设定它循环的频率

	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. 打开~/catkin_ws/src/test_pkg/CMakelist.txt
   添加add_executable(turtle_test src/turtle_test.cpp)，
   target_link_libraries(turtle_test ${catkin_LIBRARIES})两行代码在

###########
## Build ##
###########

模块后，分别表示将turtle_test.cpp编译成名为turtle_test的可执行文件；并把${catkin_LIBRARIES}指向的库（即：可执行二进制代码）链接到它身上。

4. cd ~/catkin_ws，catkin_make等待编译完成 这里注意，编译一定是要进入工作空间的根目录下，也就是~/catkin_ws，再执行source devel/setup.bash确保系统环境变量可以找到你的代码
5. 进行实验：打开一个新终端，启动ROS masterroscore，再打开一个新终端，启动turtle_noderosrun turtlesim turtlesim_node，启动成功时，屏幕上出现一个乌龟窗口，接下来调用上述产生的功能包，再打开一个新终端rosrun test_pkg turtle_test。

注意，运行的trutle_test存放在devel/lib/test_pkg下

6. 看到乌龟在做匀速圆周运动，成功。

实验1(python语言版本)

注意点：在使用python语言版本的程序时，程序脚本需要有可执行权限，可以使用chomd +x 文件名赋予可执行权限

#!/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

有一个问题，只要你将source ~/catkin_ws/devel/setup.bash添加进了.bashrc文件里，终端就可以无视路径，随时随地就可以直接调用功能包了。

实验2：订阅者Subscriber编程

运行海龟仿真器，作为Publisher发布数据，实验编写Subscriber达到订阅海龟Pose信息。
话题名/turtle1/pose。
订阅信息：turtlesim::Pose
框架图：

实验2(C语言版本)

步骤：

1. cd ~/catkin_ws/src/test_pkg/src进入功能包存放代码的地方，touch turtle_test_subscriber.cpp
2. 打开turtle_test_subscriber.cpp文件，复制下方代码，粘贴进去，保存并关闭文件

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

// 接收到订阅的消息后，会进入消息回调函数
void poseCallback(const turtlesim::Pose::ConstPtr& msg) //turtlesim::Pose是订阅的话题发布的信息类型，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);
    // 队列长度为10，作为接收到信息的缓冲区大小，poseCallback是回调函数
    //回调函数的目的是：订阅者不知道发布者什么时候会发信息过来，所以需要有回调函数，起到一个监听的作用

    // 循环等待调用回调函数
    ros::spin();
    //持续查看队列是否有消息，有消息就调用回调函数；没有就继续等待，阻塞在此处。

    return 0;
}

需要注意：回调函数里的操作一定要是高效的，如果不高效的话，当新消息来了，也不会再调用回调函数进行处理，会卡在里面。即回调函数不会嵌套调用。

3. 打开~/catkin_ws/src/test_pkg/CMakelist.txt
   添加add_executable(turtle_test_subscriber src/turtle_test_subscriber.cpp)，
   target_link_libraries(turtle_test_subscriber ${catkin_LIBRARIES})两行代码在

###########
## Build ##
###########

模块后，分别表示将turtle_test_subscriber.cpp编译成名为turtle_test_subscriber的可执行文件；并把${catkin_LIBRARIES}指向的库（即：可执行二进制代码）链接到它身上。

4. cd ~/catkin_ws，catkin_make等待编译完成 这里注意，编译一定是要进入工作空间的根目录下，也就是~/catkin_ws，再执行source devel/setup.bash确保系统环境变量可以找到你的代码
5. 进行实验：打开一个新终端，启动ROS masterroscore，再打开一个新终端，启动turtle_noderosrun turtlesim turtlesim_node，启动成功时，屏幕上出现一个乌龟窗口，接下来调用上述产生的功能包，再打开一个新终端rosrun turtlesim turtle_teleop_key，再打开一个新终端rosrun test_pkg turtle_test_subscriber。
6. 通过上下左右操作海龟运动，turtle_test_subscriber实时打印出海龟坐标位置，成功。

实验2(python语言版本)

注意点：在使用python语言版本的程序时，程序脚本需要有可执行权限，可以使用chomd +x 文件名赋予可执行权限

使用python脚本，只需要将.py脚本拷贝到test_pkg/src中，给予可执行权限，直接用rosrun test_pkg 脚本.py即可。

#!/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()