《动手学ROS2》4.3ROS2话题通信(C++)

本系列教程作者：小鱼
公众号：鱼香ROS
QQ交流群：139707339
教学视频地址：小鱼的B站
完整文档地址：鱼香ROS官网
版权声明：如非允许禁止转载与商业用途。

4.2.2 ROS2话题通信（C++实现）

本节小鱼将带你一起实现C++版本话题通信功能，帮助单身汉王二拿到他最想看的艳娘传奇，并通过话题支付稿费给李四。最终实现像下图这样的计算图。

1.订阅话题

1.1 如何编写

在C++中如何实现话题的订阅呢？其实做法和Python中差不多。还记得第三章中我们新建的SingleDogNode类的定义吗？

class SingleDogNode : public rclcpp::Node

SingleDogNode继承了Node节点，其实Node可以理解为一个类，SingleDogNode继承了Node后就会拥有Node类所具备的能力和知识。

和python中WriterNode继承的Node类一样，本节中会使用SingleDogNode继承而来的三种能力：

1. 创建一个话题订阅者的能力，用于拿到艳娘传奇的数据

   rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub_;
   sub_ = this->create_subscription<std_msgs::msg::String>("sexy_girl", 10, std::bind(&SingleDogNode::topic_callback, this, _1));
   

2. 创建一个话题发布者的能力，用于给李四送稿费

   rclcpp::Publisher<std_msgs::msg::UInt32>::SharedPtr pub_;
   pub_ = this->create_publisher<std_msgs::msg::UInt32>("sexy_girl_money",10);
   

3. 获取日志打印器的能力

   // 打印一句自我介绍
   RCLCPP_INFO(this->get_logger(), "大家好，我是单身汉王二.");
   

更多的能力可以参考rclcpp的API：https://docs.ros2.org/foxy/api/rclcpp/index.html

1.2 代码实现

创建话题订阅者的一般流程：

扫描二维码关注公众号，回复： 13231475 查看本文章

1. 导入订阅的话题接口类型
2. 创建订阅回调函数
3. 声明并创建订阅者
4. 编写订阅回调处理逻辑

用VsCode打开上一章中town_ws工作空间，并打开wang2.cpp。我们在其中添加代码即可。

添加完成后SingleDogNode类中代码如下：

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


using std::placeholders::_1;
using std::placeholders::_2;

/*
    创建一个类节点，名字叫做SingleDogNode,继承自Node.
*/
class SingleDogNode : public rclcpp::Node
{

public:
    // 构造函数,有一个参数为节点名称
    SingleDogNode(std::string name) : Node(name)
    {
        // 打印一句自我介绍
        RCLCPP_INFO(this->get_logger(), "大家好，我是单身狗%s.", name.c_str());
         // 创建一个订阅者来订阅李四写的小说，通过名字sexy_girl
        sub_novel = this->create_subscription<std_msgs::msg::String>("sexy_girl", 10, std::bind(&SingleDogNode::topic_callback, this, _1));
    }

private:
    // 声明一个订阅者（成员变量）,用于订阅小说
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub_novel;

    // 收到话题数据的回调函数
    void topic_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::init(argc, argv);
    /*产生一个Wang2的节点*/
    auto node = std::make_shared<SingleDogNode>("wang2");
    /* 运行节点，并检测退出信号*/
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

1.3代码讲解

对比python，我们使用C++订阅话题，步骤基本相同，需要添加对应的消息类型头文件：

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

创建订阅者和发布者时依然使用this->create_subscription和this->create_publisher方法。

C++中创建一个订阅者，需要传入话题类型、话题名称、所要绑定的回调函数，以及通信Qos（这里我们写10即可，后面小鱼会给大家讲解）。

std::bind()
C++的类成员函数不能像普通函数那样用于回调，因为每个成员函数都需要有一个对象实例去调用它。
通常情况下，要实现成员函数作为回调函数：一种过去常用的方法就是把该成员函数设计为静态成员函数（因为类的成员函数需要隐含的this指针 而回调函数没有办法提供），但这样做有一个缺点，就是会破坏类的结构性，因为静态成员函数只能访问该类的静态成员变量和静态成员函数，不能访问非静态的，要解决这个问题，可以把对象实例的指针或引用做为参数传给它。
后面就可以靠这个对象实例的指针或引用访问非静态成员函数。另一种办法就是使用std::bind和std::function结合实现回调技术。

2.订阅测试

完成上面的代码后，我们就可以进行测试了。

2.1 编译运行

colcon build --packages-select village_wang

source运行

source install/setup.bash 
ros2 run  village_wang  wang2_node 

2.2 运行李四节点

使用Ctrl+Shift+5切分一个终端出来，输入下面命令：

source install/setup.bash 
ros2 run  village_li  li4_node 

运行结果

3.发布（支付）稿费

李四也要过日子的，过日子就需要money。王二也不能白看，所以王二需要支付稿费。我们上一节中已经给李四新建了订阅者订阅话题sexy_girl_money来收取稿费了。现在我们要给王二写一个发布者，来送稿费给李四，鼓励一下李四继续创作。

3.1 如何发布

C++中创建一个发布者也比较简单，使用this->create_publisher即可创建一个发布者。

pub_ = this->create_publisher<std_msgs::msg::UInt32>("sexy_girl_money",10);

这里提供了三个参数，分别是该发布者要发布的话题名称（sexy_girl_money）、发布者要发布的话题类型（std_msgs::msg::UInt32）、Qos（10）

3.2 代码编写

我们来看一下增加发布者之后的完整代码

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


using std::placeholders::_1;
using std::placeholders::_2;

/*
    创建一个类节点，名字叫做SingleDogNode,继承自Node.
*/
class SingleDogNode : public rclcpp::Node
{

public:
    // 构造函数,有一个参数为节点名称
    SingleDogNode(std::string name) : Node(name)
    {
        // 打印一句自我介绍
        RCLCPP_INFO(this->get_logger(), "大家好，我是单身狗%s.", name.c_str());
         // 创建一个订阅者来订阅李四写的小说，通过名字sexy_girl
        sub_novel = this->create_subscription<std_msgs::msg::String>("sexy_girl", 10, std::bind(&SingleDogNode::topic_callback, this, _1));
        // 创建发布者
        pub_money = this->create_publisher<std_msgs::msg::UInt32>("sexy_girl_money", 10);
    }

private:
    // 声明一个订阅者（成员变量）,用于订阅小说
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub_novel;

    // 声明一个发布者（成员变量）,用于给李四钱
    rclcpp::Publisher<std_msgs::msg::UInt32>::SharedPtr pub_money;

    // 收到话题数据的回调函数
    void topic_callback(const std_msgs::msg::String::SharedPtr msg)
    {
        // 新建一张人民币
        std_msgs::msg::UInt32 money;
        money.data = 10;
        // 发送人民币给李四
        pub_money->publish(money);

        RCLCPP_INFO(this->get_logger(), "朕已阅：'%s'，打赏李四：%d 元的稿费", msg->data.c_str(), money.data);
    };

};

程序主要将回调函数修改了一下，每次收到了小说之后，王二就会发10块钱到sexy_girl_money话题上。

4.发布测试

4.1 编译运行王二

colcon build --packages-select village_wang

source运行

source install/setup.bash 
ros2 run  village_wang  wang2_node 

4.2运行李四节点

source install/setup.bash 
ros2 run  village_li  li4_node 

4.3 查看计算图

运行过程中，我们打开新的终端，输入rqt_graph。这时可以看到李四和王二之间的所有关系。

rqt_graph

5.总结

通过本节的学习，我们学会如何使用C++来发布或者订阅一个话题的数据了。下一节小鱼会提供一个练习示例，快来动手尝试吧。

作者介绍：

我是小鱼，机器人领域资深玩家，现深圳某独脚兽机器人算法工程师一枚
初中学习编程，高中开始接触机器人，大学期间打机器人相关比赛实现月入2W+（比赛奖金）
目前在输出机器人学习指南、论文注解、工作经验，欢迎大家关注小智，一起交流技术，学习机器人