ROS学习笔记（二）：ROS话题的发布与订阅

第十讲  发布者Publisher的编程
1. 创建功能包 learning_topic 
$ cd ~/test1/src 
$ catkin_create_pkg learning_topic std_msgs rospy roscpp geometry_msgs turtlesim

2.执行C++文件程序如下：放在创建的功能包的/learning_topic/src里 程序名为velocity_publisher.cpp

#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; //用于管理API资源，发布者

    // 创建一个Publisher，发布名为/turtle1/cmd_vel的topic，消息类型（数据结构）为geometry_msgs::Twist，"/turtle1/cmd_vel"：发布的话题名，队列长度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; //创建Twist消息类对象
        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;
}

2.如何实现一个发布者（发布到ros自带节点 ）
（1）初始化ROS节点
（2）向ROS Master注册节点信息，包括发布的话题名和话题中的消息类型
（3）创建消息数据
（4）按照一定频率循环发送消息
（5）如何配置CMakeList.txt中的编译规则
     设置需要编译的代码和生成的可执行文件
     设置链接库
     将下面两句话复制到功能包learning_topic里面的CMakeList.txt，放在文件中## Build ##的下面，## Install ##的上面   中间需要空一行
     add_executable(velocity_publisher src/velocity_publisher.cpp) #把哪一个cpp文件编译成那一个可执行文件

     target_link_libraries(velocity_publisher ${catkin_LIBRARIES}) #把可执行文件和ROS相关库作链接

 （6）编译 $ cd ~/test1  (回到工作空间的根目录)
          $ catkin_make (出现100%说明编译成功)
  （7）设置环境变量 （在主文件夹下（带下载，图片的那个）按ctrl + H，显示隐藏文件.bashrc）
                  终端输入：gedit ~/.bashrc
          – 文件打开后直接翻到最后面
          – 在底部添加source ~/test1/devel/setup.bash  （test1替换掉）路径写错在下一步打开终端时会报错
          – 保存退出即可（通过该方法设置的环境变量以后执行该工作空间的程序时都不用再添加了）
  （8）运行节点(重新打开三个终端，依次运行三个节点) $ roscore    $ rosrun turtlesim turtlesim_node $ rosrun  learning_topic velocity_publisher
  （9）编译生成的 velocity_publisher 在~/test1/devel/lib/learning_topic下  

3.执行Python文件
在learning_topic文件夹下创建一个文件夹scripts来存储.py程序 程序名为velocity_publisher.py,然后到scripts文件夹内将.py文件右键->属性->权限->允许作为程序执行文件 打钩

程序如下：

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

        
运行节点(重新打开三个终端，依次运行三个节点) $ roscore    $ rosrun turtlesim turtlesim_node  $rosrun learning_topic 双击Tab键会出现.py文件， $ rosrun learning_topic elocity_publisher.py 直接运行节点
 
第十一讲  订阅者Subscrier的编程1. 将下面程序放在创建的功能包的/learning_topic/src里 命名为：pose_subscriber.cpp
/**
 * 该例程将订阅/turtle1/pose话题，消息类型turtlesim::Pose
 */
 

#include <ros/ros.h>
#include "turtlesim/Pose.h"

// 接收到订阅的消息后，会进入消息回调函数
void poseCallback(const turtlesim::Pose::ConstPtr& 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);

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

    return 0;
}

2.如何实现一个订阅者
（1）初始化ROS节点
（2）订阅需要的话题
（3）循环等待话题消息，接收到消息后进入回调函数（类似中断）
（4）在回调函数中完成消息处理
（5）配置CMakeList.txt中的编译规则
     设置需要编译的代码和生成的可执行文件
     设置链接库
     将下面两句话复制到功能包learning_topic里面的CMakeList.txt，放在文件中## Build ##的下面，## Install ##的上面
     add_executable(pose_subscriber src/pose_subscriber.cpp) #把哪一个cpp文件编译成那一个可执行文件
     target_link_libraries(pose_subscriber ${catkin_LIBRARIES}) #把可执行文件和ROS相关库作链接
 （6）编译 $ cd ~/test1  (回到工作空间的根目录)
          $ catkin_make (出现100%说明编译成功)
  （7）设置环境变量 （在主文件夹下（带下载，图片的那个）按ctrl + H，显示隐藏文件.bashrc）
                  终端输入：gedit ~/.bashrc
          – 文件打开后直接翻到最后面
          – 在底部添加source ~/test1/devel/setup.bash  （test1替换掉）路径写错在下一步打开终端时会报错
          – 保存退出即可（通过该方法设置的环境变量以后执行该工作空间的程序时都不用再添加了）
  （8）运行节点(重新打开三个终端，依次运行三个节点) $ roscore    $ rosrun turtlesim turtlesim_node $ rosrun  learning_topic velocity_publisher
  （9）编译生成的 velocity_publisher 在~/test1/devel/lib/learning_topic下 

# 该例程将订阅/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()

运行节点(重新打开四个终端，依次运行四个节点) $ roscore    $ rosrun turtlesim turtlesim_node  $rosrun learning_topic 双击Tab键会出现.py文件， 
                                          $ rosrun learning_topic elocity_publisher.py   $ rosrun learning_topic pose_subscriber.py 直接运行节点  

第十二讲  话题消息的自定义与使用（！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！）

1. 如何自定义话题消息
Person.msg 文件内容如下需要根据不同的语言配置数据类型，下面是C++ 

string name
uint8 sex
uint8 age

uint8 unknown = 0  相当于宏定义，可以利用这些宏来表达性别
uint8 male = 1
uint8 female = 2

（1）定义msg文件
    在learning_topic文件夹内自定义文件夹msg，存放与消息相关的定义
    终端在msg文件夹内打开，利用 $ touch Person.msg 创建消息文件，然后将自定义数据粘贴进去
    string name
    uint8 sex
    uint8 age

    uint8 unknown = 0  
    uint8 male = 1
    uint8 female = 2
    
（2）在learning_topic中的package.xml中添加功能包依赖，动态生成程序，放到最下面有一对类似的放一起 （<export>的上面）
    <build_depend>message_generation</build_depend>
    <exec_depend>message_runtime</exec_depend>
    
（3）在learning_topic中的CMakeList.txt添加编译选项 
      1.在find_package（最上面）中加入message_generation功能包
     find_package(…… message_generation)
    
      2.添加编译FILES Person.msg的依赖，将下面添加到generate_messages的下面（## Declare ROS dynamic reconfigure parameters ##的上面）
    add_message_files(FILES Person.msg)
    generate_messages(DEPENDENCIES std_msgs)
    
      3.创建message运行的依赖,将catkin_package中的CATKIN_DEPENDS那一行取消注释，然后添加到后面    
    catkin_package(…… message_runtime)
（4）编译生成语言相关文件 回到工作空间目录，利用$ catkin_make 编译,生成的文件在devel/include/learning_topic/Person.h

（5）创建发布者/订阅者代码C++  /**
 * 该例程将发布/person_info话题，自定义消息类型learning_topic::Person   代码名称：person_publisher.cpp
 */
 

#include <ros/ros.h>
#include "learning_topic/Person.h"

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

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

    // 创建一个Publisher，发布名为/person_info的topic，消息类型为learning_topic::Person，队列长度10
    ros::Publisher person_info_pub = n.advertise<learning_topic::Person>("/person_info", 10);

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

    int count = 0;
    while (ros::ok())
    {
        // 初始化learning_topic::Person类型的消息
        learning_topic::Person person_msg;
        person_msg.name = "Tom";
        person_msg.age  = 18;
        person_msg.sex  = learning_topic::Person::male;

        // 发布消息
        person_info_pub.publish(person_msg);

           ROS_INFO("Publish Person Info: name:%s  age:%d  sex:%d", 
                  person_msg.name.c_str(), person_msg.age, person_msg.sex);

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

    return 0;
}

/**
 * 该例程将订阅/person_info话题，自定义消息类型learning_topic::Person  代码名称：person_subscriber.cpp
 */
 

#include <ros/ros.h>
#include "learning_topic/Person.h"

// 接收到订阅的消息后，会进入消息回调函数
void personInfoCallback(const learning_topic::Person::ConstPtr& msg)
{
    // 将接收到的消息打印出来
    ROS_INFO("Subcribe Person Info: name:%s  age:%d  sex:%d", 
             msg->name.c_str(), msg->age, msg->sex);
}

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

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

    // 创建一个Subscriber，订阅名为/person_info的topic，注册回调函数personInfoCallback
    ros::Subscriber person_info_sub = n.subscribe("/person_info", 10, personInfoCallback);

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

    return 0;
}

（6）编译上面的两个C++文件，将下面6句添加到learning_topic中的CMakeList.txt, 放在文件中## Build ##的下面，## Install ##的上面

     add_executable(person_publisher src/person_publisher.cpp) 
     target_link_libraries(person_publisher ${catkin_LIBRARIES}) 
     add_dependencies(person_publisher ${PROJECT_NAME}_generate_messages_cpp)
     
     add_executable(person_subscriber src/person_subscriber.cpp) 
     target_link_libraries(person_subscriber ${catkin_LIBRARIES}) 
     add_dependencies(person_subscriber ${PROJECT_NAME}_generate_messages_cpp)
     
 （7）依次打开三个终端运行三个节点，$ roscore  $ rosrun learning_topic person_subscriber  $ rosrun learning_topic person_publisher    （PS：先运行person_subscriber节点让它等待）