ROS 四：发布者与订阅者的实现
程序目标：
实现一个Publisher，发布速度指令的msg，控制turtlesim运动。
第一步：创建一个功能包
功能包相关请参考：https://tuoyou-hao.github.io/2022/07/17/ros-3-workspace-pkg/
进入工作空间的src路径
catkin_create_pkg learn_topic rospy roscpp std_msgs geometry_msgs turtlesim一、如何创建一个publisher
建立一个publisher的基本流程
graph TB a(初始化节点) a -->b(注册节点) b -->c(创建消息) c -->d(按一定频率发布消息)
c++代码实现
一、编写代码
先创建一个cpp文件touch publisher.cpp
我们将使用代码发布turtle1/cmd_vel话题，消息类型为geometry_msgs::Twist
- 首先，导入头文件
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>然后编写我们的主函数：
int main(int argc, char **argv){}- 主函数里，先初始化节点
// ROS节点初始化,节点名字叫publisher，唯一的名字
ros::init(argc, argv, "publisher");argc, argv是主函数传入的参数
publisher是节点的名称，注意不能重名
- 创建节点句柄
// 创建节点句柄,变量名为n
ros::NodeHandle n;节点句柄是用来管理ros相关的api资源的。比如创建发布者，或创建一系列api调用，都需要用ros的句柄来调用。
-
创建publisher来发布消息
向ROS Master注册节点信息，包括发布的话题名和话题的消息类型。
// 创建一个Publisher,叫turtle_vel_pub
// 发布名为/turtle1/cmd_vel的topic
// 消息类型为geometry_msgs::Twist，队列长度10
ros::Publisher turtle_vel_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);在n.advertise的尖括号里要放的是数据类型，后面跟随要发布的topic名以及消息队列长度。
-
消息队列长度有一点要注意的是：如果发送频率太高，以太网或底层系统可能来不及快速响应，所以用一个队列，先把消息放在队列里，然后再对外发送数据。
-
如果这里的队列爆满了，队列会把先进队的数据丢弃，队列会永远保存最晚进队列的数据。所以队列会出现掉帧、掉数据的情况。
- 设置循环频率
// 设置循环的频率
ros::Rate loop_rate(10);-
循环发送数据
先初始化消息，然后让他按照一定频率发布消息。
int count = 0;
while (ros::ok())
{
// 初始化geometry_msgs::Twist类型的消息
geometry_msgs::Twist vel_msg;
vel_msg.linear.x = 0.5; //线速度
vel_msg.angular.z = 0.2; //角速度
// 发布消息
turtle_vel_pub.publish(vel_msg);
ROS_INFO("Publish command[%0.2f m/s, %0.2f rad/s]", vel_msg.linear.x, vel_msg.angular.z);
// 按照循环频率延时
loop_rate.sleep();
}二、代码编译
在功能包里的CMakeLists里增加两句代码：
###########
## Build ##
###########
#在CMakeList里的build一栏底下补充上：
##add_executable(可执行文件名 要编译的cpp文件)
add_executable(publisher src/publisher.cpp)
##链接ros的库
##target_link_libraries(可执行文件 链接的库)
##这里我们链接catkin_LIBRARIES
target_link_libraries(publisher ${catkin_LIBRARIES})在终端输入以下指令进行编译：
cd ~/ros_ws
catkin_make
source devel/setup.bash运行代码测试效果：
roscore
rosrun turtlesim turtlesim_node
rosrun learn_topic publisher如果命令行报错显示找不到功能包：
[rospack] Error: package 'learn_topic' not found记得检查ROS_PACKAGE_PATH环境变量，是否运行source devel/setup.bash。
python代码实现
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
from geometry_msgs.msg import Twist
def publisher():
# ROS节点初始化
rospy.init_node('publisher', anonymous=True)
# 创建一个Publisher
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:
publisher()
except rospy.ROSInterruptException:
pass编程思想和c++开发的一样。
在运行roscore和turtlesim的前提下，直接用python指令运行代码即可
二、如何创建一个订阅者
开发目标：订阅turtlesim的pose信息
c++代码实现
头文件
#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, "subscriber");
// 创建节点句柄
ros::NodeHandle n;
// 创建一个Subscriber，订阅名为/turtle1/pose的topic，注册回调函数poseCallback
ros::Subscriber pose_sub = n.subscribe("/turtle1/pose", 10, poseCallback);
// 循环等待回调函数
ros::spin();
return 0;
}编写完毕后，和上述一样，进行编译，然后rosrun运行
python实现
# -*- coding: utf-8 -*-
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 subscriber():
# ROS节点初始化
rospy.init_node('subscriber', anonymous=True)
# 创建一个Subscriber，订阅名为/turtle1/pose的topic，注册回调函数poseCallback
rospy.Subscriber("/turtle1/pose", Pose, poseCallback)
# 循环等待回调函数
rospy.spin()
if __name__ == '__main__':
subscriber()代码参考自：公众号古月居
