ROS2——手把手教你编写一个话题

话题简介

ROS2将复杂的机器人系统拆解成许多模块节点，而这些节点之间则是通过一个至关重要的通道完成数据交换的，这个通道就是“话题”。

一个节点可以通过多个话题向外发布数据，也可以同时订阅多个其他节点发布的话题，相当于话题是一个多对多的订阅/发布模型。

可见，话题是节点之间实现数据传输的重要途径，也是机器人各个子系统之间交换数据的重要方式。

下面, 我们将从一个实例出发, 手把手写一段话题程序

案例描述

来看一个案例:

• 有一家KFC和一个饥肠辘辘的Customer.
• Customer给KFC10元钱就能买到一个汉堡, 这个Customer饿得很快, 每秒都要吃一个汉堡.
• KFC收到Customer发来的10元钱, 向Customer发送汉堡, 为了推销大鸡腿, 每5秒发布一条广告.

以上案例实际上就实现了一个话题, KFC和Customer是两个节点Node, 付钱/发送汉堡/发送广告即为发布者, 收钱/接收汉堡/收取广告即为订阅者.

程序实现

下面来编写这段程序

新建工作空间

mkdir -p ros2_ws/src
cd ros2_ws/src

mkdir -p: 递归创建目录,即使上级目录不存在,会按目录层级自动创建目录

新建功能包

ros2 pkg create customer_and_kfc --build-type ament_cmake --dependencies rclcpp std_msgs

使用ament_cmake作为编译类型, 并使用依赖rclcpp和std_msgs

在ros2_ws/src/customer_and_kfc/src下创建KFC.cpp和Customer.cpp

编写KFC节点

直接献出程序, 每句都有注释, 看不懂你打我

// rclcpp库
#include "rclcpp/rclcpp.hpp"
// 基本消息类型库
#include "std_msgs/msg/string.hpp"
#include "std_msgs/msg/u_int32.hpp"

// 这样在下文可以使用1000ms这种表示方式
using namespace std::chrono_literals;

// 占位符,下面会详细说
using std::placeholders::_1;

// 创建一个类节点，起名叫做KFCNode,继承自Node,这样就能使用Node所有的功能了
class KFCNode : public rclcpp::Node
{
public:
    // 构造函数,第一个参数为节点名称, 并初始化count为1
    KFCNode(std::string name) : Node(name), count(1)
    {
        // 打印KFC的自我介绍
      	// c_str()函数是string类的一个函数，作用是把string类型转化为char类型(%s要求是一个字符串)
        RCLCPP_INFO(this->get_logger(), "大家好, 我是%s的服务员.",name.c_str());
        
        // 创建发布者, 发布hamburger, 发布的消息类型为<std_msgs::msg::String>
      	// 格式: 发布者名字 = this->create_publisher<要发布的话题类型>("要发布的话题名称", 通信Qos);
        pub_hamburger = this->create_publisher<std_msgs::msg::String>("hamburger", 10);
        
        // 创建发布者, 发布advertisement
        pub_advertisement = this->create_publisher<std_msgs::msg::String>("advertisement", 10);
        
        // 创建定时器,每5000ms发布一个广告
      	// 格式: 定时器名字 = his->create_wall_timer(1000ms, std::bind(&定时器回调函数, this));
        advertisement_timer = this->create_wall_timer(5000ms, std::bind(&KFCNode::advertisement_timer_callback, this));
        
        // 创建订阅者,订阅money
      	// 格式: 订阅者名字 = this->create_subscription<要订阅的话题类型>("要订阅的话题名称", 通信Qos, std::bind(&订阅者回调函数, this, _1));
      	// std::bind()是干啥的呢? 举个例子: 
      	// 		auto f = std::bind(fun, placeholders::_2, placeholders::_1, 80);
      	// 		f(60,70) 等效于 fun(70, 60, 80) 
      	// 还记得前文提到的占位符吗,placeholders::_1 就是f(60,70) 中的那个参数"1"
        sub_money = this->create_subscription<std_msgs::msg::UInt32>("money_of_hamburger", 10, std::bind(&KFCNode::money_callback, this, _1));
    }
private:
    // 定义一个汉堡售出计数器
  	// 在32位系统中size_t是4字节的，在64位系统中，size_t是8字节的，这样利用该类型可以增加程序移植性。
    size_t count;

    // 声明一个定时器
    rclcpp::TimerBase::SharedPtr advertisement_timer;

    // 声明一个发布者,用于发布汉堡
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_hamburger;
    
    // 声明一个订阅者,用于收钱
    rclcpp::Subscription<std_msgs::msg::UInt32>::SharedPtr sub_money;

    // 声明一个发布者,用于发布广告
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_advertisement;

    // 广告定时器回调函数(无参数)
    void advertisement_timer_callback()
    {
      	// 定义一个String类型的字符串, 其中字符串存在.data中, %s使用时别忘了使用.c_str()转换为char类型.
        auto str_advertisement = std_msgs::msg::String();
        str_advertisement.data = "大鸡腿降价啦";
        RCLCPP_INFO(this->get_logger(), "KFC发布了一个广告:%s", str_advertisement.data.c_str());
        pub_advertisement->publish(str_advertisement);
    }
    
    // 收钱订阅者回调函数(有参数, 参数类型跟上面订阅者订阅的参数类型相同, 注意要加上::SharedPtr, 因为传进来的是一个指针)
    void money_callback(const std_msgs::msg::UInt32::SharedPtr msg)
    {
        // 如果收到了十元钱,才发布汉堡. 订阅的信息在msg->data中
        if(msg->data == 10)
        {
            RCLCPP_INFO(this->get_logger(), "收款 %d 元", msg->data);

            // 字符串流
            auto str_hamburger_num = std_msgs::msg::String();
            str_hamburger_num.data = "第" + std::to_string(count++) + "个汉堡";
            RCLCPP_INFO(this->get_logger(), "这是我卖出的%s", str_hamburger_num.data.c_str());
            
            // 发布字符串流
          	// 发布就这么写 "发布器->publish(要发布的);", 简单吧
            pub_hamburger->publish(str_hamburger_num);
        }
        
    }
};

int main(int argc, char **argv)
{
    // 初始化rclcpp
    rclcpp::init(argc, argv);
    // 产生一个KFC的节点
    auto node = std::make_shared<KFCNode>("KFC");
  	// spin函数: 一旦进入spin函数，相当于它在自己的函数里面死循环了。只要回调函数队列里面有callback函数在，它就会马上去执行callback函数。如果没有的话，它就会阻塞，不会占用CPU。注意不要再spin后面放其他东西, 他们都不会执行的
    rclcpp::spin(node);
  	// 检测退出信号(ctrl+c)
    rclcpp::shutdown();
    return 0;
}

编写Customer节点

此段程序与上面相同的语句不再解释, 请读者自行类比

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "std_msgs/msg/u_int32.hpp"

// 这样就能使用1000ms这种表示方式
using namespace std::chrono_literals;

//占位符
using std::placeholders::_1;
using std::placeholders::_2;

// 创建一个类节点，名字叫做CustomerNode,继承自Node.
class CustomerNode : public rclcpp::Node
{
public:
    // 构造函数,第一个参数为节点名称
    CustomerNode(std::string name) : Node(name)
    {
        // 打印Customer的自我介绍
        RCLCPP_INFO(this->get_logger(), "大家好,我是一个%s.",name.c_str());

        // 创建订阅者,订阅hamburger
        // 占位符还记得吗? 复习一下, 此处的_1 表示const std_msgs::msg::String::SharedPtr msg
        sub_hamburger = this->create_subscription<std_msgs::msg::String>("hamburger", 10, std::bind(&CustomerNode::hamburger_callback, this, _1));
        
        // 创建订阅者,订阅advertisement
        sub_advertisement = this->create_subscription<std_msgs::msg::String>("advertisement", 10, std::bind(&CustomerNode::advertisement_callback, this, _1));
        
        // 创建定时器,每1000ms饿一次
        hungry_timer = this->create_wall_timer(1000ms, std::bind(&CustomerNode::hungry_timer_callback, this));

        // 创建发布者,发布money
        pub_money = this->create_publisher<std_msgs::msg::UInt32>("money_of_hamburger", 10);
        
        // 给money赋值
        money.data = 10;

        //第一次给钱
        pub_money->publish(money);
        RCLCPP_INFO(this->get_logger(), "我饿了, 我要吃汉堡! 付款 %d 元", money.data);

    }
private:
    // 新建一张钱
    std_msgs::msg::UInt32 money;

    // 声明一个定时器
    rclcpp::TimerBase::SharedPtr hungry_timer;

    // 声明一个订阅者,用于订阅发出的汉堡
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub_hamburger;

    // 声明一个发布者,用于给KFC钱
    rclcpp::Publisher<std_msgs::msg::UInt32>::SharedPtr pub_money;

    // 声明一个订阅者,用于订阅广告
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub_advertisement;
    
    // 汉堡订阅者回调函数
    void hamburger_callback(const std_msgs::msg::String::SharedPtr msg)
    {
        RCLCPP_INFO(this->get_logger(), "这是我吃的 %s ", msg->data.c_str());
    }

    // 饥饿定时器回调函数
    void hungry_timer_callback()
    {
        RCLCPP_INFO(this->get_logger(), "我又饿了, 还想再吃一个! 付款 %d 元", money.data);
        pub_money->publish(money);
    }

    // 广告订阅者回调函数
    void advertisement_callback(const std_msgs::msg::String::SharedPtr msg)
    {
        RCLCPP_INFO(this->get_logger(), "我收到了一条广告: %s ", msg->data.c_str());
    }
};

int main(int argc, char **argv)
{
    //初始化rclcpp
    rclcpp::init(argc, argv);
    //产生一个Customer的节点
    auto node = std::make_shared<CustomerNode>("Customer");
    //运行节点，并检测退出信号
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

Cmakelist.txt

如果新建功能包的时候没有加--dependencies rclcpp std_msgs等功能包, 则需要手动添加: (任意位置均可)

find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)

下面两对代码的作用是:

add_executable() 让编译器编译Customer.cpp和KFC.cpp这两个文件. 并生成可执行文件Customer_node和KFC_node

ament_target_dependencies 添加编译的依赖

add_executable(Customer_node src/Customer.cpp)
ament_target_dependencies(Customer_node rclcpp std_msgs)

add_executable(KFC_node src/KFC.cpp)
ament_target_dependencies(KFC_node rclcpp std_msgs)

将编译好的文件安装到install/customer_and_kfc/lib/customer_and_kfc下

install(TARGETS
  Customer_node
  KFC_node
  DESTINATION lib/${PROJECT_NAME}
)

package.xml

同样地, 新建功能包的时候没有加--dependencies rclcpp std_msgs等功能包, 则需要手动添加, 放置于<package>标签下

<depend>rclcpp</depend>
<depend>std_msgs</depend>

也可自行修改下面这些声明, 与实现功能无关, 但是最好写全

<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="fanziqi@fanziqi.site">fanziqi</maintainer>
<license>TODO: License declaration</license>

编译

--packages-select指定编译customer_and_kfc功能包

colcon build --packages-select customer_and_kfc 

刷新环境

echo "source /ros2_ws/install/setup.zsh" >> ~/.bashrc  
source ~/.bashrc

运行

新建一个终端窗口, 运行Customer节点

ros2 run customer_and_kfc Customer_node

再另新建一个终端, 运行KFC节点

ros2 run customer_and_kfc KFC_node

此时应该可以看见:

Customer端:

KFC端:

经验证, 需求全部实现~

相关工具

rqt_graph

使用rqt_graph这个工具可以可视化显示节点和话题的连接关系

另起一个终端, 输入

rqt_graph

上图清晰地展示了ROS计算图的网络形态, 可以清楚地看出一个节点的输入和输出是什么.

ros2 topic

查看系统中所有话题

ros2 topic list

想具体查看每个话题传输的数据类型, 则添加-t

ros2 topic list -t

输出实时话题内容

ros2 topic echo /hamburger 

查看主题信息

ros2 topic info /hamburger

查看话题的数据类型

节点之间要想成功建立数据传输，必须发布和订阅同样数据类型的消息，发布者发布的是速度指令，订阅者想订阅位置信息可是行不通的。

上文用ros2 topic list -t查看得知, /advertisement的类型为std_msgs/msg/String

通过如下指令查看这个数据类型的具体数据结构

ros2 interface show std_msgs/msg/String

可以看到, std_msgs/msg/String中包含了string data

发布一个话题消息

ros2 topic pub /test_topic std_msgs/msg/String 'data: "123"'

查看某一个话题的发布频率

ros2 topic hz /hamburger

更多教程欢迎进入我的博客学习~

标签：std,hamburger,msgs,手把手,rclcpp,advertisement,msg,编写,ROS2
来源： https://blog.csdn.net/oXiaoLingTong/article/details/120594908