ROS2——手把手教你编写一个话题
话题简介
ROS2将复杂的机器人系统拆解成许多模块节点,而这些节点之间则是通过一个至关重要的通道完成数据交换的,这个通道就是“话题”。
一个节点可以通过多个话题向外发布数据,也可以同时订阅多个其他节点发布的话题,相当于话题是一个多对多的订阅/发布模型。
可见,话题是节点之间实现数据传输的重要途径,也是机器人各个子系统之间交换数据的重要方式。
下面, 我们将从一个实例出发, 手把手写一段话题程序
案例描述
来看一个案例:
- 有一家
KFC
和一个饥肠辘辘的Customer
. -
Customer
给KFC
10元钱就能买到一个汉堡, 这个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