ROS话题通信c++和python实现

机器人是一种高度复杂的系统性实现，在机器人上可能集成各种传感器(雷达、摄像头、GPS...)以及运动控制实现，为了解耦合，在ROS中每一个功能点都是一个单独的进程，每一个进程都是独立运行的。更确切的讲，ROS是进程（也称为Nodes）的分布式框架。因为这些进程甚至还可分布于不同主机，不同主机协同工作，从而分散计算压力。

不过随之也有一个问题: 不同的进程是如何通信的？也即不同进程间如何实现数据交换的？在此我们就需要介绍一下ROS中的通信机制了。

ROS 中的基本通信机制主要有如下三种实现策略:

①话题通信(发布订阅模式)

②服务通信(请求响应模式)

③参数服务器(参数共享模式)

1、话题通信

话题通信是ROS中使用频率最高的一种通信模式，话题通信是基于发布订阅模式的，也即:一个节点发布消息，另一个节点订阅该消息。话题通信的应用场景也极其广泛，比如下面一个常见场景:

机器人在执行导航功能，使用的传感器是激光雷达，机器人会采集激光雷达感知到的信息并计算，然后生成运动控制信息驱动机器人底盘运动。

在上述场景中，就不止一次使用到了话题通信。

• 以激光雷达信息的采集处理为例，在 ROS 中有一个节点需要发布当前雷达采集到的数据，导航模块中也有节点会订阅并解析雷达数据。

• 再以运动消息的发布为例，导航模块会根据传感器采集的数据时时的计算出运动控制信息并发布给底盘，底盘也可以有一个节点订阅运动信息并最终转换成控制电机的脉冲信号。

以此类推，像雷达、摄像头、GPS.... 等等一些传感器数据的采集，也都是使用了话题通信，换言之，话题通信适用于不断更新的数据传输相关的应用场景。（机械臂仿真把图像传给OpenCV）

话题通信实现模型是比较复杂的，该模型如下图所示,该模型中涉及到三个角色:

ROS Master (管理者)

Talker (发布者)

Listener (订阅者)

ROS Master 负责保管 Talker 和 Listener 注册的信息，并匹配话题相同的 Talker 与 Listener，帮助 Talker 与 Listener 建立连接，连接建立后，Talker 可以发布消息，且发布的消息会被Listener 订阅。

0.Talker注册

Talker启动后，会通过RPC在 ROS Master 中注册自身信息，其中包含所发布消息的话题名称。ROS Master 会将节点的注册信息加入到注册表中。

1.Listener注册

Listener启动后，也会通过RPC在 ROS Master 中注册自身信息，包含需要订阅消息的话题名。ROS Master 会将节点的注册信息加入到注册表中。

2.ROS Master实现信息匹配

ROS Master 会根据注册表中的信息匹配Talker 和 Listener，并通过 RPC 向 Listener 发送 Talker 的 RPC 地址信息。

3.Listener向Talker发送请求

Listener 根据接收到的 RPC 地址，通过 RPC 向 Talker 发送连接请求，传输订阅的话题名称、消息类型以及通信协议(TCP/UDP)。

4.Talker确认请求

Talker 接收到 Listener 的请求后，也是通过 RPC 向 Listener 确认连接信息，并发送自身的 TCP 地址信息。

5.Listener与Talker件里连接

Listener 根据步骤4 返回的消息使用 TCP 与 Talker 建立网络连接。

6.Talker向Listener发送消息

连接建立后，Talker 开始向 Listener 发布消息。

注意1:上述实现流程中，前五步使用的 RPC协议，最后两步使用的是 TCP 协议

注意2: Talker 与 Listener 的启动无先后顺序要求

注意3: Talker 与 Listener 都可以有多个

注意4: Talker 与 Listener 连接建立后，不再需要 ROS Master。也即，即便关闭ROS Master，Talker 与 Listern 照常通信。

ROS中的通信方式中，topic是常用的一种。对于实时性、周期性的消息，使用topic来传输是最佳的选择。topic是一种点对点的单向通信方式，这里的“点”指的是node，也就是说node之间可以通过topic方式来传递信息。topic要经历下面几步的初始化过程：首先，publisher节点和subscriber节点都要到节点管理器进行注册，然后publisher会发布topic，subscriber在master的指挥下会订阅该topic，从而建立起sub-pub之间的通信。注意整个过程是单向的。

其中在话题通信中值得注意的是

Subscriber接收消息会进行处理，一般这个过程叫做回调(Callback)。所谓回调就是提前定义好了一个处理函数（写在代码中），当有消息来就会触发这个处理函数，函数会对消息进行处理

ROS是一种分布式的架构，一个topic可以被多个节点同时发布，也可以同时被多个节点接收。

话题通信基本操作（C++）

发布方：

# include "ros/ros.h"
# include "std_msgs/String.h"//普通文本类型的消息
# include <sstream>

int main(int argc, char **argv)
// argc和argv参数在用命令行编译程序时有用。main( int argc, char* argv[], char **env ) 中 
// 第一个参数，int型的argc，为整型，用来统计程序运行时发送给main函数的命令行参数的个数，在VS中默认值为1。 
// 第二个参数，char*型的argv[]，为字符串数组，用来存放指向的字符串参数的指针数组，每一个元素指向一个参数。各成员含义如下： 
// argv[0]指向程序运行的全路径名 
// argv[1]指向在DOS命令行中执行程序名后的第一个字符串 
// argv[2]指向执行程序名后的第二个字符串 
// argv[3]指向执行程序名后的第三个字符串 
// argv[argc]为NULL 
// 第三个参数，char**型的env，为字符串数组。
// env[]的每一个元素都包含ENVVAR=value形式的字符串，其中ENVVAR为环境变量，value为其对应的值。平时使用到的比较少。
{
// 1.设置编码 会在终端输出信息 防止乱码
setlocale(LC_ALL,"");

// 2.初始化ROS节点:命名（唯一）
// 参数1和参数2，后期为节点传值会使用
// 参数3是节点名称，是一个标识符，需要保证运行后，在ROS网络拓扑中保持唯一
// 从名字很好理解，在程序初始化ros，这行代码也是你几乎所有节点都需要的．
// talker就是node(节点)的名字．即你这段程序在ROS当中的名字叫talker
ros::init(argc,argv,"talker");

//3.实例化ROS句柄
ros::NodeHandle nh;//该类封装了ros中的一些常用功能 这句话是句柄的相对名字是相对于node_namespace

//4.实例化 发布者 对象
//泛型：发布的消息类型
//参数1：队列中最大保存的消息数，超出此阈值时，先进的先销毁（时间早的先销毁）
//n.advertise通过NodeHandle的对象告诉ROS系统我要创建一个可以发布信息的对象了。这个信息是什么类型的呢？
//<std_msgs::String>告诉ROS我要发布的是标准信息中的String类型。
//那些信息叫啥名字呢？名字叫chatter。这个chatter就是我们之前提到的topic！
//10这个数字的意思是要缓冲的信息数量。
// 为什么需要缓冲呢？我们说这是个发布信息的程序，我们还需要写一个接收信息的程序。
// 这发布和接收之间并不是瞬间进行的。我记得刚学ROS的时候老师让我们测试过发布消息和接收到消息之间的时间差。
// 既然有时间差，打个比方1ms接收一条信息，如果我的信息产生地很快，是0.1ms发布一条信息，那么接收方反应不过来了。
// 这时候咋办？ROS会把发布的信息都写进缓冲区，先存个10条，然后接收的程序慢慢从缓冲区里读，只要信息不超过10条，
// 总归是可以慢慢读完的。那么超过1000条了呢？最早的信息就直接丢掉了。缓冲区接收最新的信息放到信息序列的最后。
// 即缓冲区的信息的数据结构是queue。第一条来的信息在序列满了的情况下会被第一个丢弃。
ros::Publisher pub = nh.advertise<std_msgs::String>("chatter",10);

//5.组织被发布的数据，并编写逻辑发布数据
//定义了std_msgs::String的对象msg. 这是我们要发布的信息。
// 可能有些同学会奇怪，我们要发布的是一个string类型的变量。C++中string类型的变量，在我们包含了头文件<string>之后，
// 应该用类似于std::string st的命令来建立一个string类下的对象st。为什么ROS不直接发布那样一个对象却要发布一个"ROS里的string"呢？
// 其实ROS为了管理方便，把所有可能发布的消息都归类到ROS中了。比如你想发布8位的整型变量，你不能直接定义一个int a然后发布a。
// 你得用std_msgs::Int8 a发布这个a才行。那我怎么知道我想发布的变量应该怎么定义呢? 比如float, double 甚至机器人的pose，一张图片? 
// 这个我们必须得从ROS wiki中找到我们想使用的变量该怎么定义。下一章我们会讲解怎么使用ROS wiki找到这些信息。
// 另外如果你想发布的信息类型ROS里面没有，你可以自己定义，这个可能以后会讲到
std_msgs::String msg;

//msg.data = "你好啊！！！"
std::string msg_front = "Hello 你好！";//消息前缀
int count = 0;//消息计数器

//逻辑(一秒10次) 可以制定循环的频率
//程序如果在不断地发布信息，那么有时候我会想控制发布的信息的快慢，这行表示你希望你发布信息的速度为10Hz。
//这个函数要和ros::Rate r.sleep()配合使用才能达到控制速度目的
ros::Rate r(1);

//节点不死 要是ros不OK，程序就退出了。什么时候ROS不OK呢？
// 上面提供的中文链接中说了四种可能。最常见的就是你按下ctrl+c或者在程序遇到ros::shutdown()这行命令。
while (ros::ok())
{
//使用 stringstream 拼接字符串与编号
std::stringstream ss;
ss << msg_front <<count;
msg.data = ss.str();
//发布消息
pub.publish(msg);
//加入调试，打印发送的信息
ROS_INFO("发送的信息：%s",msg.data.c_str());

//根据前面制定的发送频率自动休眠 休眠时间 = 1/频率；
r.sleep();
count++;//循环结束前，让count 自增
//暂无应用 
// 简单来说这个函数是用于接收器的，必须要有spinOnce或者spin，ROS才会检测是不是接收到信息。
// spinOnce就是检测一次，spin()就是一直检测。但是我们这儿是发布器呀，所以它在这儿没用
ros::spinOnce();
}

}

 订阅方

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

// 我们看回调函数。不熟悉C++的同学可能要会想为什么用这个函数的参数长这个样子。 
// (const std_msgs::String::ConstPtr& msg)什么鬼? 首先我得说，这个模式还是比较固定的，
// 如果你要接受的是Int8类型的消息，那么参数会变成(const std_msgs::Int8::ConstPtr& msg)。
// ConstPtr代表一个指针。所以你目前要知道的是msg这个参数是你接收到的消息的指针就行了，同样这个名字你也随意改，
// 但一般是..msg。所以你看到printf(ROS_INFO)中有msg->data这种调用方式。（
// 在Publisher中我们定义了std_msgs::String对象msg，类包含数据成员data，调用方式为msg.data。
// 如果类的指针叫msg,那么调用该成员的方式是msg->data）。所以现在msg->data就是一个std::string类型的量，
// 假设有string类型的变量st要想print出来,代码就是printf("%s,", st.c_str() )（不能直接print st）。
// 使用ROS_INFO其内部完全一样。

void chatterCallback(const std_msgs::String::ConstPtr& msg_p)
{
ROS_INFO("我听见：%s",msg_p->data.c_str());
//ROS_INFO("我听见:%s",(*msg_p).data.c_str());
}
int main(int argc, char *argv[])
{
// 1.设置编码 会在终端输出信息 防止乱码
setlocale(LC_ALL,"");

// 2.初始化ROS节点:命名（唯一）
// main函数的头两行，和发布器的头两行一样的。
// 除了节点的名字换成了listener。名字自然是要换的，节点的名字得unique。
// 如果ROS遇到了相同的节点名字那么他会停止掉旧节点的名字然后使用新节点的那个程序
// (这时候旧的节点如果 有ros::ok()，那么他会就变得不OK了 = = 。这是ros::ok()返回false的第三种情况)
ros::init(argc,argv,"listener");

//3.实例化ROS句柄
ros::NodeHandle nh;

//4.实例化 发布者 对象
// ros::Subscriber这一行是定义接收器的方法，对比发布器中定义ros::Publisher的方法，
// 发现十分类似。Publisher使用'n.advertise'，这儿使用n.subscriber表示定义接收器, 
// chatter即前面publisher的topic的名字。注意node的名字得独一无二但是topic的名字得和你想接收的信息的topic一样！ 
// 这个很好理解，ROS怎么知道你想接收什么信息呢？如果你有两个发布器，都发布std_msgs::String类型的消息，
// 接收器通过找谁的topic和自己一样就接收谁的信息。
// 我们发现这儿有publisher中类似于<std_msgs::String>的东西来定义要接收的数据类型。其实是有的，
// 它藏在了第三个参数chatterCallback里。
// 我们注意到chatterCallback和main函数之前定义那个返回值为空的函数名字一样。
// chatterCallback称为回调函数，接收器每一次接收到消息，就会调用名字为它的函数。
// chatterCallback这个名字也是可以随意定义的，只要和前面那个返回值为空的函数有一样的名字就可以了，
// 但一般命名为...Callback这样一看就知道这是ROS使用得回调函数。ROS的回调函数返回值只能为空。
// 如果你想在接收到消息处理后返回什么有用的值怎么办呢？自然是写到类中了(当然也可以用全局变量)。这个以后讲。
ros::Subscriber sub = nh.subscribe<std_msgs::String>("chatter",10,chatterCallback);

//5.处理订阅的消息(回调函数)
// ros::spin()会使程序在循环中，一直检测有没有接收到新的消息。其终止方式和使ros::ok()返回false方式一样。
ros::spin();//循环读取接收的数据，并调用回调函数处理
return 0;
}
配置CMakeLists.txt
add_executable(demo01_pub src/demo01_pub.cpp)
add_executable(demo01_sub src/demo01_sub.cpp)

target_link_libraries(demo01_pub
${catkin_LIBRARIES}
)
target_link_libraries(demo01_sub
${catkin_LIBRARIES}
)

 Python代码

发布方：
 

#! /usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
from std_msgs.msg import String

if __name__ == "__main__":
# 2.初始化ROS节点：命名（唯一）
rospy.init_node("talker_p")
# 3.实例化 发布者 对象
pub = rospy.Publisher("chatter",String,queue_size=10)
# 4.组织被发布的数据，并编写逻辑发布数据
msg = String() # 创建 msg 对象
msg_front = "hello 你好"
count = 0 # 计数器
rate = rospy.Rate(1)
rospy.sleep(1)
while not rospy.is_shutdown():
msg.data = msg_front + str(count)

pub.publish(msg)
rate.sleep()
rospy.loginfo("写出的数据：%s",msg.data)
count += 1

 订阅方：

#! /usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
from std_msgs.msg import String

def doMsg(msg):
rospy.loginfo("I heard:%s",msg.data)

if __name__ == "__main__":
# 初始化ros节点
rospy.init_node("listener_p")
# 实例化 订阅者 对象
sub = rospy.Subscriber("chatter",String,doMsg,queue_size=10)
# 设置循环回调函数
rospy.spin()

添加python可执行权限

终端下进入 scripts 执行:chmod +x *.py

配置CMakeLists.txt

catkin_install_python(PROGRAMS
scripts/demo01_pub.py
scripts/demo01_sub.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

话题通信自定义msg

在 ROS 通信协议中，数据载体是一个较为重要组成部分，ROS 中通过 std_msgs 封装了一些原生的数据类型,比如:String、Int32、Int64、Char、Bool、Empty.... 但是，这些数据一般只包含一个 data 字段，结构的单一意味着功能上的局限性，当传输一些复杂的数据，比如: 激光雷达的信息... std_msgs 由于描述性较差而显得力不从心，这种场景下可以使用自定义的消息类型

msgs只是简单的文本文件，每行具有字段类型和字段名称，可以使用的字段类型有：

• int8, int16, int32, int64 (或者无符号类型: uint*)

• float32, float64

• string

• time, duration

• other msg files

• variable-length array[] and fixed-length array[C]

ROS中还有一种特殊类型：Header，标头包含时间戳和ROS中常用的坐标帧信息。会经常看到msg文件的第一行具有Header标头。

1.定义msg文件

功能包下新建 msg 目录，添加文件 Person.msg

string name
uint16 age
float64 height

2.编辑配置文件

package.xml中添加编译依赖与执行依

  <build_depend>message_generation</build_depend>
  <exec_depend>message_runtime</exec_depend>
  <!-- 
  exce_depend 以前对应的是 run_depend 现在非法
  -->

CMakeLists.txt编辑 msg 相关配置 

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  message_generation
)
# 需要加入 message_generation,必须有 std_msgs

 配置msg文件

add_message_files(
  FILES
  Person.msg
)

  生成消息时依赖于 std_msgs

generate_messages(
  DEPENDENCIES
  std_msgs
)

 执行时依赖

catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES demo02_talker_listener
  CATKIN_DEPENDS roscpp rospy std_msgs message_runtime
#  DEPENDS system_lib
)

此时编译空间可以在devel文件找到include文件，然后看到Person.h文件

python文件是在devel的lib文件夹查看

首先进行vscode配置

为了方便代码提示以及避免误抛异常，需要先配置 vscode，将前面生成的 head 文件路径配置进 c_cpp_properties.json 的 includepath属性:

{
    "configurations": [
        {
            "browse": {
                "databaseFilename": "",
                "limitSymbolsToIncludedHeaders": true
            },
            "includePath": [
                "/opt/ros/noetic/include/**",
                "/usr/include/**",
                "/xxx/yyy工作空间/devel/include/**" //配置 head 文件的路径 
            ],
            "name": "ROS",
            "intelliSenseMode": "gcc-x64",
            "compilerPath": "/usr/bin/gcc",
            "cStandard": "c11",
            "cppStandard": "c++17"
        }
    ],
    "version": 4
}

 

C++程序

发布方：

# include "ros/ros.h"
# include "pub_sub_test/Person.h"

int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");

// 初始化ROS节点
ros::init(argc,argv,"talker_person");
// 创建ROS句柄
ros::NodeHandle nh;

// 创建发布者对象
ros::Publisher pub = nh.advertise<pub_sub_test::Person>("chatter",1000);

// 组织被发布的消息，编写发布逻辑并发布信息
pub_sub_test::Person p;
p.name = "zhangsan";
p.age = 23;
p.height = 1.67;

ros::Rate r(1);
while (ros::ok())
{
pub.publish(p);
p.age += 1;
ROS_INFO("我叫：%s,今年%d岁,高%.2f米",p.name.c_str(),p.age,p.height);
r.sleep();
//建议，此处没用
ros::spinOnce();
}
return 0;
}

订阅方：
 

# include "ros/ros.h"
# include "pub_sub_test/Person.h"

void chatterCallback(const pub_sub_test::Person::ConstPtr& person_p)
{
ROS_INFO("订阅人的信息：%s,%d,%.2f",(*person_p).name.c_str(),person_p->age,person_p->height);
}
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
ros::init(argc,argv,"listener_person");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe("chatter",1000,chatterCallback);

ros::spin();

return 0;
}

配置 CMakeLists.txt

需要添加 add_dependencies 用以设置所依赖的消息相关的中间文件。

add_executable(demo02_pub_person src/demo02_pub_person.cpp)
add_executable(demo02_sub_person src/demo02_sub_person.cpp)

add_dependencies(demo02_pub_person ${PROJECT_NAME}_generate_messages_cpp)
add_dependencies(demo02_sub_person ${PROJECT_NAME}_generate_messages_cpp)
# 此处保证先编译msg产生Person.h中间文件，然后编译demo02_pub_person

target_link_libraries(demo02_pub_person
${catkin_LIBRARIES}
)
target_link_libraries(demo02_sub_person
${catkin_LIBRARIES}
)

Python程序

首先进行vscode配置

为了方便代码提示以及误抛异常，需要先配置 vscode，将前面生成的 python 文件路径配置进 settings.json

{
    "python.autoComplete.extraPaths": [
        "/opt/ros/noetic/lib/python3/dist-packages",
        "/xxx/yyy工作空间/devel/lib/python3/dist-packages"
    ]
}

 

 发布方：

#! /usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
from pub_sub_test.msg import Person

if __name__ == "__main__":
# 初始化ros节点
rospy.init_node("talker_person_p")
# 创建发布者对象
pub = rospy.Publisher("chatter_person",Person,queue_size=10)
# 组织消息
p = Person()
p.name = "葫芦瓦"
p.age = 18
p.height = 0.75

# 编写消息发布逻辑
rate = rospy.Rate(1)
while not rospy.is_shutdown():
pub.publish(p)
rate.sleep()
p.age += 1
rospy.loginfo("姓名：%s,年龄：%d，身高:%.2f",p.name,p.age,p.height)

 订阅方：

#! /usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
from pub_sub_test.msg import Person

def doPerson(p):
rospy.loginfo("接收到的人的信息：%s,%d,%.3f", p.name,p.age,p.height)

if __name__ == "__main__":
# 初始化ros节点
rospy.init_node("listener_person_p")
# 创建订阅者对象
sub = rospy.Subscriber("chatter_person",Person,doPerson,queue_size=10)
rospy.spin()