ROS入门-8.发布者Publisher的编程实现

主要实现功能是：让我们在海龟仿真器里的海龟通过程序控制动起来
之前过程中，主要是通过命令行或者通过键盘控制形式来发布速度指令
以下是该话题通讯的问题模型

今天学习怎样通过一个程序来实现其中的发布者，其发布海龟的速度指令让海龟能够动起来
模型概述：
该话题模型里有ROS Master，用来去管理所有话题节点；该系统当中有两个主要节点，一个是Subscriber,即海龟仿真器turtlesim，另外一个是我们现在要实现的Publisher，即为Turtle Velocity这样一个速度指令的一个发布者，他要通过程序去实现它并且来发布一个message, message的数据结构是Twist这样一个消息，里面分线速度和角速度，通过topic这样一个总线管道去把数据传输给Subscriber，Subscriber订阅得到数据来控制海龟发生运动
下面通过程序实现功能

第一步，在工作空间里src文件里创建功能包

catkin_create_pkg learning_topic roscpp rospy std_msgs geometry_msgs turtlesim

1）功能包里面放置后面我们要实现的代码
2）即创建学习topice相关功能的编程的一个功能包，记住功能包一定要放到工作空间里的src里面才可以
3）cd ~/catkin_ws/src
4）catkin_create_pkg learning_topic(功能包名)+添加依赖 roscpp(依赖c++的库) rospy(依赖python的库) std_msgs(标准的消息) geometry_msgs(Twist消息所在的包) turtlesim(很多数据上的定义是在turtlesim里做定义的)

第二步，实现publisher/发布者的代码，以C++为例

初始化ROS节点
向ROS Master注册节点信息，包括发布的话题名和话题中的消息类型
创建消息数据
按照一定频率循环发布消息

在learning_topic里src里创建cpp文件，即c++代码文件

/**
 * 该例程将发布turtle1/cmd_vel话题，里面会包含消息的类型即数据结构为geometry_msgs::Twist   该消息主要是用来控制海龟完成速度指令的运动的
 */
 
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
//包含的头文件，ros相关的头文件，后面用到的很多的函数定义都是在ros.h里定义的；另外一个是包含Twist，就是线速度和角速度相关的消息的定义的头文件，这是在geometry_msgs这个库里已经定义好的，因此包含一下就好了

int main(int argc, char **argv)
{
	// ROS节点初始化，利用ros::init完成节点初始化，要设置节点的名字（这里设名字为velocity_publisher），注意节点名字不要重复；argc, argv是main函数里输入的参数，主要来完成一些可以通过输入的参数来设置一些初始化的属性，一般默认情况下这些属性没有什么配置，基本上只有这节点的名字
	//这句话就是告诉ros master，这个节点来了，要启动了
	ros::init(argc, argv, "velocity_publisher");

	// 创建节点句柄；主要用来管理ros相关的api一些资源的，比如创建发布者，创建api调用，都需要用到节点的句柄来做调用的，故主用来管理节点的资源的
	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);（括号里初始化内容分两个内容，前者参数，发布的话题的话题名，而且和turtlesim所订阅的话题名匹配，否则管道不同，数据就会传输到其他地方。现在我们要在名为/turtle1/cmd_vel话题里发布消息   后者参数10，是队列长度，主要表示在发布者Publisher发布数据的时候，底层可能没有办法来得及快速相应该发布的频率，就会把所要发布的数据先放到一个队列里来，然后不断往外发布。举例，比如publisher发布一秒钟一万次，会有一个队列，先把一万次存放到队列里面来，然后再根据实际发送的能力从队列缓存里往外发送数据，如果底层发送能力还是太弱了，ros会默认把时间最老的数据(即最先进队的数据)除去，永远保存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())
	{
	    //进入while循环，封装数据并且发布出去，延时满足所设置的频率
	    // 初始化geometry_msgs::Twist类型的消息内容

		geometry_msgs::Twist vel_msg;//创建类的对象，设置线速度，角速度
		vel_msg.linear.x = 0.5;    //线速度0.5米每秒
		vel_msg.angular.z = 0.2;    //角速度0.2弧度米每秒

	    // 发布消息
		turtle_vel_pub.publish(vel_msg);//发布数据，调用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);
				//ROS_INFO类似printf做日志输出的，告诉客户端现在信息已经发出去了，以及发布了一个怎样的信息


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

	return 0;
}

第三步.配置发布者代码编译规则

打开CMakeLists.txt，在build内，install前内容添加下面两句话

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

总述，add_executable是用来把哪一个程序文件（例：velocity_publisher.cpp）编译成哪一个可执行文件的（例：velocity_publisher）
target_link_libraries是用来帮助我们把可执行文件跟ros相关的库去做链接的，也就是我们调用的一些c++或python的一些接口，这些接口会通过target_link_libraries去跟ros相关的库去做连接
即先做代码的编译，在做这些代码的可执行链接
第四步.编译并运行发布者

cd ~/catkin_ws //回到工作空间根目录下
catkin_make //用该指令去做编译
source devel/setup.bash //设置环境变量，可通过下面方法减去该步骤
roscore
rosrun turtlesim turtlesim_node
rosrun learning_topic velocity_publisher //运行learning_topic里的一个节点，节点名为velocity_publisher

可见ros本身的环境变量，这是在安装ros时就设置的，可以按照同样的语法把工作空间的环境变量也设置进来，重新启动终端生效，这样就不用每次在运行程序的时候都在终端里面再去输入一遍source指令了，而且可以在任意路径下去运行rosrun命令

依次打开三个终端输入

roscore
rosrun turtlesim turtlesim_node
rosrun learning_topic velocity_publisher

可见海龟以线速度0.5米每秒，角速度0.2弧度每秒运动

关于我们编译生成正在运行的可执行文件的位置,即让海龟运行起来的程序：
home/catkin_ws/devel/lib/learning_topic/velocity_publisher

在ros里运行python文件要注意一定要让python文件有可执行权限，这样才可以以rosrun命令做执行，操作步骤：
右击python文件/属性/权限/√允许作为程序执行文件点成

python文件的内容（基本流程类似）
rosrun learning_topic velocity_publisher.py

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