ROS--话题通信笔记 C++与 python

原创已于 2022-03-04 22:10:17 修改 · 535 阅读

0 ·

CC 4.0 BY-SA版权

文章标签：

#ubuntu #python #自动驾驶

于 2022-03-03 16:47:19 首次发布

本文介绍了C++与Python在ROS中通过发布/订阅机制实现消息通信的过程，包括发布方的C++代码示例，如何设置定时计数，以及Python发布和订阅方的实现。同时涉及CMakeLists.txt配置和解决订阅初期数据丢失问题的方法。

1.话题通信理论模型

理论模型来源：https://www.bilibili.com/video/BV1Ci4y1L7ZZ?p=40

2.1 C++发布方框架

在这里插入图片描述
注：运行rosrun xxx 不输出时：
如“rosrun plumbing_pub_sub demo01_pub”
输入：

rostopic echo 话题名称

如：
“rostopic echo fang”
（fang为文件中定义的）

例：
在这里插入图片描述

2.2 发布与订阅

2.2.1 发布方：

#include "ros/ros.h"
#include "std_msgs/String.h"
#include <sstream>

/*
    发布方实现：
        1.包含头文件
        ROS中文本类型 -->std_msgs/String.h
        2.初始化ROS节点
        3.创建节点句柄
        4.创建发布者对象
        5.编写发布逻辑并发布数据
        6.声明spin()函数
*/

int main(int argc, char  *argv[])
{
    setlocale(LC_ALL,"");
    //2.初始化ROS节点
    ros::init(argc,argv,"erGouZi");
    //3.创建节点句柄
    ros::NodeHandle nh;
    //4.创建发布者对象
    ros::Publisher pub = nh.advertise<std_msgs::String>("fang",10);
    //5.编写发布逻辑并发布数据
    //要求以 10HZ 的频率发布数据，并且文本后添加编号
    //先创建被发布的消息
    std_msgs::String msg;
    //发布频率
    ros::Rate rate(10);
    //设置编号（添加计算器）
    int count = 0;
    //编写循环，循环中发布数据
    while (ros::ok())
    {
        count++;
        //实现字符串拼接数字
        std::stringstream ss; 
        ss <<"hello ---> " << count;
        //msg.data = "hello";
        msg.data = ss.str();
        pub.publish(msg);
        //添加日志：
        ROS_INFO("发布的数据是:%s", ss.str().c_str());
        rate.sleep();
        ros::spinOnce(); //官方建议添加，处理回调函数
    }
    return 0;
}

2.2.2 订阅方

#include "ros/ros.h"
#include "std_msgs/String.h"

/*
    订阅方实现：
        1.包含头文件
        ROS中文本类型 -->std_msgs/String.h
        2.初始化ROS节点
        3.创建节点句柄
        4.创建订阅者对象
        5.处理订阅到的数据
        6.声明spin()函数
*/

void doMsg(const std_msgs::String::ConstPtr &msg){
    //通过msg获取并操作订阅到的数据
    ROS_INFO("翠花订阅的数据:%s", msg->data.c_str());
}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    //2.初始化ROS节点
    ros::init(argc,argv,"cuiHua");
    //3.创建节点句柄
    ros::NodeHandle nh;
    //4.创建订阅者对象
    ros::Subscriber sub = nh.subscribe("fang",10,doMsg);
    //5.处理订阅到的数据
    //处理回调函数
    ros::spin();  
    return 0;
}

2.3 修改CMakeLists.txt文件
add_executable与target_link_libraries：

在这里插入图片描述

2.4 终端输出：

注：在工作空间下source一下：

source ./devel/setup.bash

在这里插入图片描述
2.5 补充：
2.5.1 订阅时，前几条数据丢失
原因：
发送前几条数据时，publisher还未在roscore注册完毕
解决：
注册后加入休眠 ros::Duration(3).sleep(); 延迟第一条数据的发送(添加了3秒的失眠)

ros::Duration(3).sleep();

如：
在这里插入图片描述

3.1 python发布方框架

#! /usr/bin/env python

import rospy
from std_msgs.msg import String #发布的消息类型

"""
    使用 python 实现消息发布:
        1.导包;
        2.初始化ROS节点;
        3.创建发布者对象
        4.编写发布逻辑并发布数据
"""
if __name__ == "__main__":

        #2.初始化ROS节点;
        rospy.init_node("sanDai")  #传入节点名称
        #3.创建发布者对象
        pub = rospy.Publisher("che",String,queue_size=10)
        #4.编写发布逻辑并发布数据
        #创建数据
        msg = String()
        #使用循环发布数据
        while not rospy.is_shutdown():
            msg.data = "hello"
            #发布数据
            pub.publish(msg)

注：
3.1.1 python文件添加可执行权限：

chmod +x *.py

如：
在这里插入图片描述
3.1.2 修改CMakeLists.txt

3.1.3 输出：

3.2 设置编号计数（仅修改上述框架#4的地方）

代码：

#4.编写发布逻辑并发布数据
        #创建数据
        msg = String()
        #指定发布频率
        rate = rospy.Rate(1)
        #设置计算器
        count = 0
        #使用循环发布数据
        while not rospy.is_shutdown():
            count += 1
            msg.data = "hello" + str(count)
            #发布数据
            pub.publish(msg)
            rospy.loginfo("发布的数据:%s",msg.data)
            rate.sleep()

输出：
在这里插入图片描述

3.3 发布与订阅

3.3.1发布方：

#! /usr/bin/env python

from itertools import count
import rospy
from std_msgs.msg import String #发布的消息类型

"""
    使用 python 实现消息发布:
        1.导包;
        2.初始化ROS节点;
        3.创建发布者对象
        4.编写发布逻辑并发布数据
"""
if __name__ == "__main__":

        #2.初始化ROS节点;
        rospy.init_node("sanDai")  #传入节点名称
        #3.创建发布者对象
        pub = rospy.Publisher("che",String,queue_size=10)
        #4.编写发布逻辑并发布数据
        #创建数据
        msg = String()
        #指定发布频率
        rate = rospy.Rate(1)
        #设置计算器
        count = 0
        #使用循环发布数据
        while not rospy.is_shutdown():
            count += 1
            msg.data = "hello" + str(count)
            #发布数据
            pub.publish(msg)
            rospy.loginfo("发布的数据:%s",msg.data)
            rate.sleep()

3.3.2 订阅方

# !/uer/bin/env python
import rospy
from std_msgs.msg import String

"""
    订阅实现流程:
        1.导包;
        2.初始化ROS节点;
        3.创建一个订阅者对象;
        4.回调函数处理数据
        5.spin()

"""

def doMsg(msg):
    rospy.loginfo("我订阅的数据:%s",msg.data)


if __name__ == "__main__":

        #2.初始化ROS节点;
        rospy.init_node("huaHhua")
        #3.创建一个订阅者对象;
        sub = rospy.Subscriber("che",String,doMsg,queue_size=10)
        #4.回调函数处理数据
        #5.spin()
        rospy.spin()
pass