第二章、ROS通信机制

• 话题通信(发布订阅模式)
• 服务通信(请求响应模式)
• 参数服务器(参数共享模式)

• 能够熟练介绍ROS中常用的通信机制
• 能够理解ROS中每种通信机制的理论模型
• 能够以代码的方式实现各种通信机制对应的案例
• 能够熟练使用ROS中的一些操作命令
• 能够独立完成相关实操案例

1.话题演示案例:

2.服务演示案例:在指定位置生成乌龟

**3.参数演示案例:改变乌龟窗口的背景颜色**

1、话题通讯（已验证）

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

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

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

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

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

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

概念

以发布订阅的方式实现不同节点之间数据交互的通信模式。

作用

用于不断更新的、少逻辑处理的数据传输场景。

案例（下面讲述）

1.实现最基本的发布订阅模型，发布方以固定频率发送一段文本，订阅方接收文本并输出。(2.1.2 — 2.1.3)

2.实现对自定义消息的发布与订阅。(2.1.4 — 2.1.6)

1、话题通信理论模型

• ROS Master (管理者)
• Talker (发布者)
• Listener (订阅者)

0.Talker注册

1.Listener注册

2.ROS Master实现信息匹配

3.Listener向Talker发送请求

4.Talker确认请求

5.Listener与Talker件里连接

6.Talker向Listener发送消息

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

需求:

分析:

1. **<font style="color:rgb(51, 51, 51);">发布方</font>**
2. **<font style="color:rgb(51, 51, 51);">接收方</font>**
3. **<font style="color:rgb(51, 51, 51);">数据(此处为自定义消息</font>**<font style="color:rgb(51, 51, 51);">)</font>

流程:

1. <font style="color:rgb(51, 51, 51);">编写发布方实现；</font>
2. <font style="color:rgb(51, 51, 51);">编写订阅方实现；</font>
3. <font style="color:rgb(51, 51, 51);">编辑配置文件；</font>
4. <font style="color:rgb(51, 51, 51);">编译并执行。</font>

1.发布方

/*
    需求: 实现基本的话题通信，一方发布数据，一方接收数据，
         实现的关键点:
         1.发送方
         2.接收方
         3.数据(此处为普通文本)
         PS: 二者需要设置相同的话题
    消息发布方:
        循环发布信息:HelloWorld 后缀数字编号
    实现流程:
        1.包含头文件 
            ros中的文本类型---<std_msgs/String.h>
        2.初始化 ROS 节点:命名(唯一)
        3.实例化 ROS 句柄
        4.实例化 发布者 对象
        5.组织被发布的数据，并编写逻辑发布数据
*/
// 1.包含头文件 
#include "ros/ros.h"
#include "std_msgs/String.h" //普通文本类型的消息
#include <sstream>
int main(int argc, char  *argv[])
{   
    //设置编码
    setlocale(LC_ALL,"");
    //2.初始化 ROS 节点:命名(唯一)
    // 参数1和参数2 后期为节点传值会使用
    // 参数3 是节点名称，是一个标识符，需要保证运行后，在 ROS 网络拓扑中唯一
    ros::init(argc,argv,"talker");
    //3.实例化 ROS 句柄
    ros::NodeHandle nh;//该类封装了 ROS 中的一些常用功能
    //4.实例化 发布者 对象
    //泛型: 发布的消息类型std_msgs::String
    //参数1: 要发布到的话题"chatter"
    //参数2: 队列中最大保存的消息数，超出此阀值时，先进的先销毁(时间早的先销毁)
    ros::Publisher pub = nh.advertise<std_msgs::String>("chatter",10);
    //5.组织被发布的数据，并编写逻辑发布数据
    //数据(动态组织)
    std_msgs::String msg;
    // msg.data = "你好啊！！！";
    std::string msg_front = "Hello 你好！"; //消息前缀
    int count = 0; //消息计数器
    //逻辑(一秒10次)
    ros::Rate r(1); //注2
    //节点不死
    while (ros::ok())   //ok
    {
        //使用 stringstream 拼接字符串与编号
        std::stringstream ss; //实例化
        ss << msg_front << count;  //将msg_front和count进行拼接
        msg.data = ss.str();
        //发布消息
        pub.publish(msg);
        //加入调试，打印发送的消息
        ROS_INFO("发送的消息:%s",msg.data.c_str());  //见注1
        //根据前面制定的发送贫频率自动休眠 休眠时间 = 1/频率；
        r.sleep(); 
        count++;//循环结束前，让 count 自增
        //暂无应用
        ros::spinOnce();  //注3
    }
    return 0;
}

注：

（1）c_str() 以 char 形式传回 string 内含字符串
如果一个函数要求char参数，可以使用c_str()方法：

c_str()函数返回一个指向正规C字符串的指针常量, 内容与本string串相同.
这是为了与c语言兼容，在c语言中没有string类型，故必须通过string类对象的成员函数c_str()把string 对象转换成c中的字符串样式。

1.c_str是一个内容为字符串指向字符数组的临时指针；
**2.c_str返回的是一个可读不可改的常指针；**

string s = "Hello World!";
printf("%s", s.c_str()); //输出 "Hello World!"

（2）ros::Rate xx 是一个定时器函数。单位是hz。通过xx.sleep（）休眠来度过剩下的时间。

（3）ros::spin（）与ros::spinOnce（） 两者都叫**消息回调处理函数。两者区别在于前者调用后不会再返回，也就是你的主程序到这儿就不往下执行了，而后者在调用后还可以继续执行之后的程序。如果你的程序写了相关的消息订阅函数，那么程序在执行过程中，除了主程序以外，ROS还会自动在后台按照你规定的格式，接受订阅的消息，但是所接到的消息并不是立刻就被处理，而是必须要等到ros::spin()或ros::spinOnce()执行的时候才被调用，这就是消息回到函数的原理**

(4)rostopic list

rostopic echo 话题名称 （在屏幕上打印该话题消息）

2.订阅方

/*
    需求: 实现基本的话题通信，一方发布数据，一方接收数据，
         实现的关键点:
         1.发送方
         2.接收方
         3.数据(此处为普通文本)
    消息订阅方:
        订阅话题并打印接收到的消息
    实现流程:
        1.包含头文件 
        2.初始化 ROS 节点:命名(唯一)
        3.实例化 ROS 句柄
        4.实例化 订阅者 对象
        5.处理订阅的消息(回调函数)
        6.设置循环调用回调函数
*/
// 1.包含头文件 
#include "ros/ros.h"
#include "std_msgs/String.h"
//回调函数  （参数是订阅到的消息） 消息是std_msgs::String类型，ConstPt常量指针的引用
        //msg_p是指针变量
void doMsg(const std_msgs::String::ConstPtr& msg_p){
    //通过msg获取并操作订阅到的数据
    ROS_INFO("我听见:%s",msg_p->data.c_str());
    // ROS_INFO("我听见:%s",(*msg_p).data.c_str());
}
int main(int argc, char  *argv[])
{
    setlocale(LC_ALL,"");
    //2.初始化 ROS 节点:命名(唯一)
    ros::init(argc,argv,"listener");
    //3.实例化 ROS 句柄
    ros::NodeHandle nh;
    //4.实例化 订阅者 对象  （话题chatter要和发布方的一致）
    ros::Subscriber sub = nh.subscribe<std_msgs::String>("chatter",10,doMsg);
    //5.处理订阅的消息(回调函数)
    //     6.设置循环调用回调函数
    ros::spin();//循环读取接收的数据，并调用回调函数处理
    return 0;
}

注：关键点在于chatter这个话题要保持一致！！！！

3.配置 CMakeLists.txt

add_executable(Hello_pub
  src/Hello_pub.cpp
)
add_executable(Hello_sub
  src/Hello_sub.cpp
)
target_link_libraries(Hello_pub
  ${catkin_LIBRARIES}
)
target_link_libraries(Hello_sub
  ${catkin_LIBRARIES}
)

4.执行

1.启动 roscore;

2.启动发布节点;

3.启动订阅节点。

5.注意

补充0:

补充1:

You may notice that sometimes your project builds fine even if you did not call find_package with all dependencies. This is because catkin combines all your projects into one, so if an earlier project calls find_package, yours is configured with the same values. But forgetting the call means your project can easily break when built in isolation.

补充3:

或者

2.2、话题通信基本操作B（Py）

流程:

1. 编写发布方实现；
2. 编写订阅方实现；
3. 为python文件添加可执行权限；
4. 编辑配置文件；
5. 编译并执行。

1.发布方

#! /usr/bin/env python
"""
    需求: 实现基本的话题通信，一方发布数据，一方接收数据，
         实现的关键点:
         1.发送方
         2.接收方
         3.数据(此处为普通文本)
         PS: 二者需要设置相同的话题
    消息发布方:
        循环发布信息:HelloWorld 后缀数字编号
    实现流程:
        1.导包 
        2.初始化 ROS 节点:命名(唯一)
        3.实例化 发布者 对象
        4.组织被发布的数据，并编写逻辑发布数据
"""
#1.导包 
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)
    while not rospy.is_shutdown():
        #拼接字符串
        msg.data = msg_front + str(count)
        pub.publish(msg)
        rate.sleep()
        rospy.loginfo("写出的数据:%s",msg.data)
        count += 1

2.订阅方

#! /usr/bin/env python
"""
    需求: 实现基本的话题通信，一方发布数据，一方接收数据，
         实现的关键点:
         1.发送方
         2.接收方
         3.数据(此处为普通文本)
    消息订阅方:
        订阅话题并打印接收到的消息
    实现流程:
        1.导包 
        2.初始化 ROS 节点:命名(唯一)
        3.实例化 订阅者 对象
        4.处理订阅的消息(回调函数)
        5.设置循环调用回调函数
"""
#1.导包 
import rospy
from std_msgs.msg import String
def doMsg(msg):
    rospy.loginfo("I heard:%s",msg.data)
if __name__ == "__main__":
    #2.初始化 ROS 节点:命名(唯一)
    rospy.init_node("listener_p")
    #3.实例化 订阅者 对象
    sub = rospy.Subscriber("chatter",String,doMsg,queue_size=10)
    #4.处理订阅的消息(回调函数)
    #5.设置循环调用回调函数
    rospy.spin()

3.添加可执行权限

4.配置 CMakeLists.txt

5.执行

注：c++版本发布与py版本的订阅可以兼容，只要保证话题名称一样就好。

3、话题通信自定义msg

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

• int8, int16, int32, int64 (或者无符号类型: uint*)
• float32, float64
• string
• time, duration
• other msg files
• variable-length array[] and fixed-length array[C]

需求:创建自定义消息，该消息包含人的信息:姓名、身高、年龄等。

流程:

1. **<font style="color:#F5222D;">按照固定格式创建 msg 文件</font>**
2. **<font style="color:#F5222D;">编辑配置文件</font>**
3. **<font style="color:#F5222D;">编译生成可以被 Python 或 C++ 调用的中间文件</font>**

1.定义msg文件

string name
uint16 age
float64 height

2.编辑配置文件

package.xml中添加编译依赖与执行依赖（2步）

<build_depend>message_generation</build_depend>   //编译时依赖
  <exec_depend>message_runtime</exec_depend>    //运行时依赖
  <!-- 
  exce_depend 以前对应的是 run_depend 现在非法
  -->

CMakeLists.txt编辑 msg 相关配置（4步）

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
)

3.编译

编译后的中间文件查看:

后续调用相关 msg 时，是从这些中间文件调用的

4、话题通信自定义msg调用A（C++）

流程:

1. <font style="color:rgb(51, 51, 51);">编写发布方实现；</font>
2. <font style="color:rgb(51, 51, 51);">编写订阅方实现；</font>
3. <font style="color:rgb(51, 51, 51);">编辑配置文件；</font>
4. <font style="color:rgb(51, 51, 51);">编译并执行。</font>

0.vscode 配置

{
    "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
}

1.发布方

/*
    需求: 循环发布人的信息
*/
#include "ros/ros.h"
#include "demo02_talker_listener/Person.h"  //前面就是功能包的名称
int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    //1.初始化 ROS 节点
    ros::init(argc,argv,"talker_person");
    //2.创建 ROS 句柄
    ros::NodeHandle nh;
    //3.创建发布者对象
    ros::Publisher pub = nh.advertise<demo02_talker_listener::Person>("chatter_person",1000);
    //4.组织被发布的消息，编写发布逻辑并发布消息
    demo02_talker_listener::Person p;
    p.name = "sunwukong";
    p.age = 2000;
    p.height = 1.45;
    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;
}

2.订阅方

/*
    需求: 订阅人的信息
*/
#include "ros/ros.h"
#include "demo02_talker_listener/Person.h"  //前面就是功能包的名称
//回调函数
void doPerson(const demo02_talker_listener::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,"");
    //1.初始化 ROS 节点
    ros::init(argc,argv,"listener_person");
    //2.创建 ROS 句柄
    ros::NodeHandle nh;
    //3.创建订阅对象
    ros::Subscriber sub = nh.subscribe<demo02_talker_listener::Person>("chatter_person",10,doPerson);
    //4.回调函数中处理 person
    //5.ros::spin();
    ros::spin();    
    return 0;
}

3.配置 CMakeLists.txt（自定义的消息类型有三处修改）

5、话题通信自定义msg调用B（Py）

分析:

1. <font style="color:rgb(51, 51, 51);">发布方</font>
2. <font style="color:rgb(51, 51, 51);">接收方</font>
3. <font style="color:rgb(51, 51, 51);">数据(此处为自定义消息)</font>

流程:

1. <font style="color:rgb(51, 51, 51);">编写发布方实现；</font>
2. <font style="color:rgb(51, 51, 51);">编写订阅方实现；</font>
3. <font style="color:rgb(51, 51, 51);">为python文件添加可执行权限；</font>
4. <font style="color:rgb(51, 51, 51);">编辑配置文件；</font>
5. <font style="color:rgb(51, 51, 51);">编译并执行。</font>

0.vscode配置 （自定义消息会多出这一步）

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

1.发布方

#! /usr/bin/env python
"""
    发布方:
        循环发送消息
"""
import rospy
from demo02_talker_listener.msg import Person   //前面就是功能包的名称
if __name__ == "__main__":
    #1.初始化 ROS 节点
    rospy.init_node("talker_person_p")
    #2.创建发布者对象
    pub = rospy.Publisher("chatter_person",Person,queue_size=10)
    #3.组织消息
    p = Person()
    p.name = "葫芦瓦"
    p.age = 18
    p.height = 0.75
    #4.编写消息发布逻辑
    rate = rospy.Rate(1)
    while not rospy.is_shutdown():
        pub.publish(p)  #发布消息
        rate.sleep()  #休眠
        rospy.loginfo("姓名:%s, 年龄:%d, 身高:%.2f",p.name, p.age, p.height)

2.订阅方

#! /usr/bin/env python
"""
    订阅方:
        订阅消息
"""
import rospy
from demo02_talker_listener.msg import Person  //前面就是功能包的名称
def doPerson(p):
    rospy.loginfo("接收到的人的信息:%s, %d, %.2f",p.name, p.age, p.height)
if __name__ == "__main__":
    #1.初始化节点
    rospy.init_node("listener_person_p")
    #2.创建订阅者对象
    sub = rospy.Subscriber("chatter_person",Person,doPerson,queue_size=10)
    rospy.spin() #4.循环

3.权限设置

4.配置 CMakeLists.txt （操作一致）

catkin_install_python(PROGRAMS
  scripts/talker_p.py
  scripts/listener_p.py
  scripts/person_talker.py
  scripts/person_listener.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

5.执行

2、服务通信

服务通信也是ROS中一种极其常用的通信模式，服务通信是基于请求响应模式的，是一种应答机制。也即: 一个节点A向另一个节点B发送请求，B接收处理请求并产生响应结果返回给A。比如如下场景:

机器人巡逻过程中，控制系统分析传感器数据发现可疑物体或人… 此时需要拍摄照片并留存。

概念

以请求响应的方式实现不同节点之间数据交互的通信模式。

作用

用于偶然的、对时时性有要求、有一定逻辑处理需求的数据传输场景。

案例

实现两个数字的求和，客户端节点，运行会向服务器发送两个数字，服务器端节点接收两个数字求和并将结果响应回客户端。

1、服务通信理论模型

- **<font style="color:rgb(51, 51, 51);">ROS master(管理者)</font>**
- **<font style="color:#F5222D;">Server(服务端)</font>**
- **<font style="color:#F5222D;">Client(客户端)</font>**

0.Server注册

1.Client注册

2.ROS Master实现信息匹配

3.Client发送请求

4.Server发送响应

2、服务通信自定义srv

流程:

    1. **<font style="color:rgb(51, 51, 51);">按照固定格式创建srv文件</font>**
    2. **<font style="color:rgb(51, 51, 51);">编辑配置文件</font>**
    3. **<font style="color:rgb(51, 51, 51);">编译生成中间文件</font>**

1.定义srv文件（和msg文件类似）

 客户端请求时发送的两个数字 
int32 num1
int32 num2
---
# 服务器响应发送的数据
int32 sum

2.编辑配置文件

（1）package.xml中添加编译依赖与执行依赖 （与msg操作一致，一个功能包下无需重复添加）

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

（2）CMakeLists.txt编辑 srv 相关配置 （与msg操作一致,少了一个执行时依赖，一个功能包下无需重复添加）

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

add_service_files(
  FILES
  AddInts.srv  
)

注意：这个地方与msg不在一个地方

#生成消息是依赖于 std_msgs
generate_messages(
  DEPENDENCIES
  std_msgs
)

3.编译

Python 需要调用的中间文件(…/工作空间/devel/lib/python3/dist-packages/包名/srv)

3、服务通信自定义srv调用 （c++）

需求:

分析:

1. <font style="color:rgb(51, 51, 51);">服务端</font>
2. <font style="color:rgb(51, 51, 51);">客户端</font>
3. <font style="color:rgb(51, 51, 51);">数据</font>

流程:

1. <font style="color:rgb(51, 51, 51);">编写服务端实现；</font>
2. <font style="color:rgb(51, 51, 51);">编写客户端实现；</font>
3. <font style="color:rgb(51, 51, 51);">编辑配置文件；</font>
4. <font style="color:rgb(51, 51, 51);">编译并执行。</font>

0.vscode配置

#### 2.客户端 python /* 需求: 编写两个节点实现服务通信，客户端节点需要提交两个整数到服务器 服务器需要解析客户端提交的数据，相加后，将结果响应回客户端， 客户端再解析 服务器实现: 1.包含头文件 2.初始化 ROS 节点 3.创建 ROS 句柄 4.创建 客户端 对象 5.请求服务，接收响应 */ // 1.包含头文件 #include "ros/ros.h" #include "demo03_server_client/AddInts.h" int main(int argc, char *argv[]) { setlocale(LC_ALL,""); // 调用时动态传值,如果通过 launch 的 args 传参，需要传递的参数个数 +3 if (argc != 3) // if (argc != 5)//launch 传参(0-文件路径 1传入的参数 2传入的参数 3节点名称 4日志路径) { ROS_ERROR("请提交两个整数"); return 1; } // 2.初始化 ROS 节点 ros::init(argc,argv,"AddInts_Client"); // 3.创建 ROS 句柄 ros::NodeHandle nh; // 4.创建 客户端 对象 <模板> ("话题名称保证和服务器端的一致") ros::ServiceClient client = nh.serviceClient<demo03_server_client::AddInts>("AddInts"); //等待服务启动成功 //方式1 ros::service::waitForService("AddInts"); //方式2 // client.waitForExistence(); // 5.组织请求数据 demo03_server_client::AddInts ai; //提交数据的是创建实例化对象 ai.request.num1 = atoi(argv[1]); ai.request.num2 = atoi(argv[2]); // 6.发送请求,返回 bool 值，标记是否成功 bool flag = client.call(ai); //call里面就传递server对象就行。客户端访问服务器并且把访问对象ai提交. //ai对象里面有请求和响应的值 //返回结果如果是true的话就表示能正常处理。 // 7.处理响应 if (flag) { ROS_INFO("请求正常处理,响应结果:%d",ai.response.sum); } else { ROS_ERROR("请求处理失败...."); return 1; } return 0; }

#### 3.配置 CMakeLists.txt (与官网上有所区别_gencpp) python add_executable(AddInts_Server src/AddInts_Server.cpp) add_executable(AddInts_Client src/AddInts_Client.cpp) add_dependencies(AddInts_Server ${PROJECT_NAME}_gencpp) add_dependencies(AddInts_Client ${PROJECT_NAME}_gencp) target_link_libraries(AddInts_Server ${catkin_LIBRARIES} ) target_link_libraries(AddInts_Client ${catkin_LIBRARIES} ) #### 4.执行

结果:

注意:

如果先启动客户端，那么会导致运行失败。解决：在ROS中已经内置了相关的函数，在客户端启动后挂起，等待服务端启动。函数调用的额时机是在客户端创建之后，请求发送之前。

优化:

在客户端发送请求前添加:**client.waitForExistence();**

或:**ros::service::waitForService(“AddInts”);**

这是一个阻塞式函数，只有服务启动成功后才会继续执行

此处可以使用 launch 文件优化，但是需要注意 args 传参特点

4、服务通信自定义srv调用 （py）

分析:

1. 服务端
2. 客户端
3. 数据

流程:

1. 编写服务端实现；
2. 编写客户端实现；
3. 为python文件添加可执行权限；
4. 编辑配置文件；
5. 编译并执行。

0.vscode配置

4.配置 CMakeLists.txt

CMakeLists.txt

catkin_install_python(PROGRAMS
  scripts/AddInts_Server_p.py 
  scripts/AddInts_Client_p.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

5.执行

流程:

• 需要先启动服务:rosrun 包名 服务
• 然后再调用客户端 :rosrun 包名 客户端 参数1 参数2

结果:

3、参数服务器

参数服务器在ROS中主要用于实现不同节点之间的数据共享。参数服务器相当于是独立于所有节点的一个公共容器，可以将数据存储在该容器中，被不同的节点调用，当然不同的节点也可以往其中存储数据，关于参数服务器的典型应用场景如下:

- <font style="color:rgb(51, 51, 51);">路径规划时，需要参考小车的尺寸，我们可以将这些尺寸信息存储到参数服务器，全局路径规划节点与本地路径规划节点都可以从参数服务器中调用这些参数</font>

概念

以共享的方式实现不同节点之间数据交互的通信模式。

作用

存储一些多节点共享的数据，类似于全局变量。

案例

实现参数增、删、改、查操作。

1、参数服务器理论模型

参数服务器实现是最为简单的，该模型如下图所示,该模型中涉及到三个角色:

• ROS Master (管理者)
• Talker (参数设置者)
• Listener (参数调用者)

1.Talker 设置参数

Talker 通过 RPC 向参数服务器发送参数(包括参数名与参数值)，ROS Master 将参数保存到参数列表中。

2.Listener 获取参数

Listener 通过 RPC 向参数服务器发送参数查找请求，请求中包含要查找的参数名。

3.ROS Master 向 Listener 发送参数值

• 32-bit integers
• booleans
• strings
• doubles
• iso8601 dates
• lists
• base64-encoded binary data
• 字典

2、参数服务操作（C++）

需求:实现参数服务器参数的增删改查操作。

• ros::NodeHandle
• ros::param

1.参数服务器新增(修改)参数

/*
    参数服务器操作之新增与修改(二者API一样)_C++实现:
    在 roscpp 中提供了两套 API 实现参数操作
    ros::NodeHandle // (第一套)
        setParam("键",值)
    ros::param   // (第二套)
        set("键","值")
    示例:分别设置整形、浮点、字符串、bool、列表、字典等类型参数
        修改(相同的键，不同的值)
*/
#include "ros/ros.h"
int main(int argc, char *argv[])
{
    ros::init(argc,argv,"set_update_param"); 初始化节点
    std::vector<std::string> stus;     //实例化消息类型成stus
    stus.push_back("zhangsan");
    stus.push_back("李四");
    stus.push_back("王五");
    stus.push_back("孙大脑袋");
    std::map<std::string,std::string> friends;
    friends["guo"] = "huang";
    friends["yuang"] = "xiao";
    //NodeHandle--------------------------------------------------------
    ros::NodeHandle nh;
    nh.setParam("nh_int",10); //整型
    nh.setParam("nh_double",3.14); //浮点型
    nh.setParam("nh_bool",true); //bool
    nh.setParam("nh_string","hello NodeHandle"); //字符串
    nh.setParam("nh_vector",stus); // vector
    nh.setParam("nh_map",friends); // map
    //修改演示(相同的键，不同的值)
    nh.setParam("nh_int",10000);
    //param--------------------------------------------------------
    ros::param::set("param_int",20);
    ros::param::set("param_double",3.14);
    ros::param::set("param_string","Hello Param");
    ros::param::set("param_bool",false);
    ros::param::set("param_vector",stus);
    ros::param::set("param_map",friends);
    //修改演示(相同的键，不同的值)
    ros::param::set("param_int",20000);
    return 0;
}

2.参数服务器获取参数

/*
    参数服务器操作之查询_C++实现:
    在 roscpp 中提供了两套 API 实现参数操作
    ros::NodeHandle
        param(键,默认值) 
            存在，返回对应结果，否则返回默认值
        getParam(键,存储结果的变量)
            存在,返回 true,且将值赋值给参数2
            若果键不存在，那么返回值为 false，且不为参数2赋值
        getParamCached键,存储结果的变量)--提高变量获取效率
            存在,返回 true,且将值赋值给参数2
            若果键不存在，那么返回值为 false，且不为参数2赋值
        getParamNames(std::vector<std::string>)
            获取所有的键,并存储在参数 vector 中 
        hasParam(键)
            是否包含某个键，存在返回 true，否则返回 false
        searchParam(参数1，参数2)
            搜索键，参数1是被搜索的键，参数2存储搜索结果的变量
    ros::param ----- 与 NodeHandle 类似
*/
#include "ros/ros.h"
int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"get_param");
    //NodeHandle--------------------------------------------------------
    /*
    ros::NodeHandle nh;
    // param 函数
    int res1 = nh.param("nh_int",100); // 键存在
    int res2 = nh.param("nh_int2",100); // 键不存在
    ROS_INFO("param获取结果:%d,%d",res1,res2);
    // getParam 函数
    int nh_int_value;
    double nh_double_value;
    bool nh_bool_value;
    std::string nh_string_value;
    std::vector<std::string> stus;
    std::map<std::string, std::string> friends;
    nh.getParam("nh_int",nh_int_value);
    nh.getParam("nh_double",nh_double_value);
    nh.getParam("nh_bool",nh_bool_value);
    nh.getParam("nh_string",nh_string_value);
    nh.getParam("nh_vector",stus);
    nh.getParam("nh_map",friends);
    ROS_INFO("getParam获取的结果:%d,%.2f,%s,%d",
            nh_int_value,
            nh_double_value,
            nh_string_value.c_str(),
            nh_bool_value
            );
    for (auto &&stu : stus)
    {
        ROS_INFO("stus 元素:%s",stu.c_str());        
    }
    for (auto &&f : friends)
    {
        ROS_INFO("map 元素:%s = %s",f.first.c_str(), f.second.c_str());
    }
    // getParamCached()
    nh.getParamCached("nh_int",nh_int_value);
    ROS_INFO("通过缓存获取数据:%d",nh_int_value);
    //getParamNames()
    std::vector<std::string> param_names1;
    nh.getParamNames(param_names1);
    for (auto &&name : param_names1)
    {
        ROS_INFO("名称解析name = %s",name.c_str());        
    }
    ROS_INFO("----------------------------");
    ROS_INFO("存在 nh_int 吗? %d",nh.hasParam("nh_int"));
    ROS_INFO("存在 nh_intttt 吗? %d",nh.hasParam("nh_intttt"));
    std::string key;
    nh.searchParam("nh_int",key);
    ROS_INFO("搜索键:%s",key.c_str());
    */
    //param--------------------------------------------------------
    ROS_INFO("++++++++++++++++++++++++++++++++++++++++");
    int res3 = ros::param::param("param_int",20); //存在
    int res4 = ros::param::param("param_int2",20); // 不存在返回默认
    ROS_INFO("param获取结果:%d,%d",res3,res4);
    // getParam 函数
    int param_int_value;
    double param_double_value;
    bool param_bool_value;
    std::string param_string_value;
    std::vector<std::string> param_stus;
    std::map<std::string, std::string> param_friends;
    ros::param::get("param_int",param_int_value);
    ros::param::get("param_double",param_double_value);
    ros::param::get("param_bool",param_bool_value);
    ros::param::get("param_string",param_string_value);
    ros::param::get("param_vector",param_stus);
    ros::param::get("param_map",param_friends);
    ROS_INFO("getParam获取的结果:%d,%.2f,%s,%d",
            param_int_value,
            param_double_value,
            param_string_value.c_str(),
            param_bool_value
            );
    for (auto &&stu : param_stus)
    {
        ROS_INFO("stus 元素:%s",stu.c_str());        
    }
    for (auto &&f : param_friends)
    {
        ROS_INFO("map 元素:%s = %s",f.first.c_str(), f.second.c_str());
    }
    // getParamCached()
    ros::param::getCached("param_int",param_int_value);
    ROS_INFO("通过缓存获取数据:%d",param_int_value);
    //getParamNames()
    std::vector<std::string> param_names2;
    ros::param::getParamNames(param_names2);
    for (auto &&name : param_names2)
    {
        ROS_INFO("名称解析name = %s",name.c_str());        
    }
    ROS_INFO("----------------------------");
    ROS_INFO("存在 param_int 吗? %d",ros::param::has("param_int"));
    ROS_INFO("存在 param_intttt 吗? %d",ros::param::has("param_intttt"));
    std::string key;
    ros::param::search("param_int",key);
    ROS_INFO("搜索键:%s",key.c_str());
    return 0;
}

3.参数服务器删除参数

/* 
    参数服务器操作之删除_C++实现:
    ros::NodeHandle
        deleteParam("键")
        根据键删除参数，删除成功，返回 true，否则(参数不存在)，返回 false
    ros::param
        del("键")
        根据键删除参数，删除成功，返回 true，否则(参数不存在)，返回 false
*/
#include "ros/ros.h"
int main(int argc, char *argv[])
{   
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"delete_param");
    ros::NodeHandle nh;
    bool r1 = nh.deleteParam("nh_int");
    ROS_INFO("nh 删除结果:%d",r1);
    bool r2 = ros::param::del("param_int");
    ROS_INFO("param 删除结果:%d",r2);
    return 0;
}

3、参数服务操作（py）

1.参数服务器新增(修改)参数

#! /usr/bin/env python
"""
    参数服务器操作之新增与修改(二者API一样)_Python实现:
"""
import rospy
if __name__ == "__main__":
    rospy.init_node("set_update_paramter_p")
    # 设置各种类型参数
    rospy.set_param("p_int",10)
    rospy.set_param("p_double",3.14)
    rospy.set_param("p_bool",True)
    rospy.set_param("p_string","hello python")
    rospy.set_param("p_list",["hello","haha","xixi"])
    rospy.set_param("p_dict",{"name":"hulu","age":8})
    # 修改
    rospy.set_param("p_int",100)

2.参数服务器获取参数

#! /usr/bin/env python
"""
    参数服务器操作之查询_Python实现:    
        get_param(键,默认值)
            当键存在时，返回对应的值，如果不存在返回默认值
        get_param_cached
        get_param_names
        has_param
        search_param
"""
import rospy
if __name__ == "__main__":
    rospy.init_node("get_param_p")
    #获取参数
    int_value = rospy.get_param("p_int",10000)
    double_value = rospy.get_param("p_double")
    bool_value = rospy.get_param("p_bool")
    string_value = rospy.get_param("p_string")
    p_list = rospy.get_param("p_list")
    p_dict = rospy.get_param("p_dict")
    rospy.loginfo("获取的数据:%d,%.2f,%d,%s",
                int_value,
                double_value,
                bool_value,
                string_value)
    for ele in p_list:
        rospy.loginfo("ele = %s", ele)
    rospy.loginfo("name = %s, age = %d",p_dict["name"],p_dict["age"])
    # get_param_cached
    int_cached = rospy.get_param_cached("p_int")
    rospy.loginfo("缓存数据:%d",int_cached)
    # get_param_names
    names = rospy.get_param_names()
    for name in names:
        rospy.loginfo("name = %s",name)
    rospy.loginfo("-"*80)
    # has_param
    flag = rospy.has_param("p_int")
    rospy.loginfo("包含p_int吗？%d",flag)
    # search_param
    key = rospy.search_param("p_int")
    rospy.loginfo("搜索的键 = %s",key)

3.参数服务器删除参数

#! /usr/bin/env python
"""
    参数服务器操作之删除_Python实现:
    rospy.delete_param("键")
    键存在时，可以删除成功，键不存在时，会抛出异常
"""
import rospy
if __name__ == "__main__":
    rospy.init_node("delete_param_p")
    try:
        rospy.delete_param("p_int")
    except Exception as e:
        rospy.loginfo("删除失败")

4、常用命令

机器人系统中启动的节点少则几个，多则十几个、几十个，不同的节点名称各异，通信时使用话题、服务、消息、参数等等都各不相同，一个显而易见的问题是: 当需要自定义节点和其他某个已经存在的节点通信时，如何获取对方的话题、以及消息载体的格式呢？

• rosnode : 操作节点
• rostopic : 操作话题
• rosservice : 操作服务
• rosmsg : 操作msg消息
• rossrv : 操作srv消息
• rosparam : 操作参数

作用

案例

1、rosnode

rosnode ping    测试到节点的连接状态     （用过√）
rosnode list    列出活动节点           （用过√）
rosnode info    打印节点信息           （用过√）
rosnode machine    列出指定设备上节点  （用过√）
rosnode kill       杀死某个节点
rosnode cleanup    清除不可连接的节点

• rosnode ping测试到节点的连接状态
• rosnode list列出活动节点
• rosnode info打印节点信息
• rosnode machine列出指定设备上的节点
• rosnode kill杀死某个节点
• rosnode cleanup清除无用节点，启动乌龟节点，然后 ctrl + c 关闭，该节点并没被彻底清除，可以使用 cleanup 清除节点

2、rostopic

rostopic包含rostopic命令行工具，用于显示有关ROS 主题的调试信息，包括发布者，订阅者，发布频率和ROS消息。它还包含一个实验性Python库，用于动态获取有关主题的信息并与之交互。

rostopic bw     显示主题使用的带宽
rostopic delay  显示带有 header 的主题延迟
rostopic echo   打印消息到屏幕
rostopic find   根据类型查找主题
rostopic hz     显示主题的发布频率
rostopic info   显示主题相关信息
rostopic list   显示所有活动状态下的主题
rostopic pub    将数据发布到主题
rostopic type   打印主题类型

• rostopic list(-v)直接调用即可，控制台将打印当前运行状态下的主题名称rostopic list -v : 获取话题详情(比如列出：发布者和订阅者个数…)
• rostopic pub可以直接调用命令向订阅者发布消息为roboware 自动生成的 发布/订阅 模型案例中的 订阅者 发布一条字符串

rostopic pub /主题名称 消息类型 消息内容
rostopic pub /chatter std_msgs gagaxixi

rostopic pub /turtle1/cmd_vel geometry_msgs/Twist
 "linear:
  x: 1.0
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 2.0"
//只发布一次运动信息
rostopic pub -r 10 /turtle1/cmd_vel geometry_msgs/Twist
 "linear:
  x: 1.0
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 2.0"
// 以 10HZ 的频率循环发送运动信息

• rostopic echo获取指定话题当前发布的消息 **打印消息到屏幕**
• rostopic info获取当前话题的小关信息消息类型发布者信息订阅者信息
• rostopic type列出话题的消息类型
• rostopic find 消息类型根据消息类型查找话题
• rostopic delay列出消息头信息
• rostopic hz列出消息发布频率
• rostopic bw列出消息发布带宽

3、rosmsg

rosmsg是用于显示有关 ROS消息类型的 信息的命令行工具。

rosmsg 演示

rosmsg show    显示消息描述
rosmsg info    显示消息信息
rosmsg list    列出所有消息
rosmsg md5    显示 md5 加密后的消息
rosmsg package    显示某个功能包下的所有消息
rosmsg packages    列出包含消息的功能包

• rosmsg list会列出当前 ROS 中的所有 msg
• rosmsg packages列出包含消息的所有包
• rosmsg package列出某个包下的所有msg

//rosmsg package 包名 
rosmsg package turtlesim

• rosmsg show 显示消息描述

//rosmsg show 消息名称
rosmsg show turtlesim/Pose
结果:
float32 x
float32 y
float32 theta
float32 linear_velocity
float32 angular_velocity

• rosmsg info作用与 rosmsg show 一样
• rosmsg md资料:http://wiki.ros.org/ROS/Technical%20Overview#Message_serialization_and_msg_MD5_sums)一种校验算法，保证数据传输的一致性

4、rosservice

rosservice包含用于列出和查询ROSServices的rosservice命令行工具。

调用部分服务时，如果对相关工作空间没有配置 path，需要进入工作空间调用 source ./devel/setup.bash

rosservice args    打印服务参数
rosservice call    使用提供的参数调用服务
rosservice find    按照服务类型查找服务
rosservice info    打印有关服务的信息
rosservice list    列出所有活动的服务
rosservice type    打印服务类型
rosservice uri    打印服务的 ROSRPC uri

• rosservice list 列出所有活动的 service

~ rosservice list
/clear
/kill
/listener/get_loggers
/listener/set_logger_level
/reset
/rosout/get_loggers
/rosout/set_logger_level
/rostopic_4985_1578723066421/get_loggers
/rostopic_4985_1578723066421/set_logger_level
/rostopic_5582_1578724343069/get_loggers
/rostopic_5582_1578724343069/set_logger_level
/spawn
/turtle1/set_pen
/turtle1/teleport_absolute
/turtle1/teleport_relative
/turtlesim/get_loggers
/turtlesim/set_logger_level

• rosservice args打印服务参数

rosservice args /spawn
x y theta name

• rosservice call 调用服务

rosservice call /spawn "x: 1.0
y: 2.0
theta: 0.0
name: 'xxx'"
name: "xxx"
//生成一只叫 xxx 的乌龟

• rosservice find根据消息类型获取话题
• rosservice info获取服务话题详情
• rosservice type获取消息类型
• rosservice uri获取服务器 uri

5、rossrv

rossrv是用于显示有关ROS服务类型的信息的命令行工具，与 rosmsg 使用语法高度雷同。

rossrv show    显示服务消息详情
rossrv info    显示服务消息相关信息
rossrv list    列出所有服务信息
rossrv md5    显示 md5 加密后的服务消息
rossrv package    显示某个包下所有服务消息
rossrv packages    显示包含服务消息的所有包

• rossrv list会列出当前 ROS 中的所有 srv 消息
• rossrv packages列出包含服务消息的所有包
• rossrv package列出某个包下的所有msg

//rossrv package 包名 
rossrv package turtlesim

• rossrv show 显示消息描述

//rossrv show 消息名称
rossrv show turtlesim/Spawn
结果:
float32 x
float32 y
float32 theta
string name
---
string name

• rossrv info作用与 rossrv show 一致
• rossrv md5对 service 数据使用 md5 校验(加密)

6、rosparam

rosparam set    设置参数
rosparam get    获取参数
rosparam load    从外部文件加载参数
rosparam dump    将参数写出到外部文件
rosparam delete    删除参数
rosparam list    列出所有参数

• rosparam list 列出所有参数

rosparam list
//默认结果
/rosdistro
/roslaunch/uris/host_helloros_virtual_machine__42911
/rosversion
/run_id

• rosparam set 设置参数

rosparam set name huluwa
//再次调用 rosparam list 结果
/name
/rosdistro
/roslaunch/uris/host_helloros_virtual_machine__42911
/rosversion
/run_id

• rosparam get 获取参数

rosparam get name
//结果
huluwa

• rosparam delete 删除参数

rosparam delete name
//结果
//去除了name

• rosparam load(先准备 yaml 文件) 从外部文件加载参数

rosparam load xxx.yaml

• rosparam dump 将参数写出到外部文件

rosparam dump yyy.yaml

5、通信机制实践操作

本节主要是通过ROS内置的turtlesim案例，结合已经介绍ROS命令获取节点、话题、话题消息、服务、服务消息与参数的信息，最终再以编码的方式实现乌龟运动的控制、乌龟位姿的订阅、乌龟生成与乌龟窗体背景颜色的修改。

1、实操——话题（topic）发布（pub）

需求描述:编码实现乌龟运动控制，让小乌龟做圆周运动。

实现分析:

1. 乌龟运动控制实现，关键节点有两个，一个是乌龟运动显示节点 turtlesim_node，另一个是控制节点，二者是订阅发布模式实现通信的，乌龟运动显示节点直接调用即可，运动控制节点之前是使用的 turtle_teleop_key通过键盘 控制，现在需要自定义控制节点。
2. 控制节点自实现时，首先需要了解控制节点与显示节点通信使用的话题与消息，可以使用ros命令结合计算图来获取。
3. 了解了话题与消息之后，通过 C++ 或 Python 编写运动控制节点，通过指定的话题，按照一定的逻辑发布消息即可。

实现流程:

1. 通过计算图结合ros命令获取话题与消息信息。
2. 编码实现运动控制节点。
3. 启动 roscore、turtlesim_node 以及自定义的控制节点，查看运行结果。

1.话题与消息获取

准备:** **先启动键盘控制乌龟运动案例。

1.1话题获取

获取话题:/turtle1/cmd_vel

rqt_graph

rostopic list

1.2消息获取

有话题意味着有消息，获取消息类型:geometry_msgs/Twist

rostopic type /turtle1/cmd_vel

获取消息格式:

rosmsg info geometry_msgs/Twist

响应结果:

geometry_msgs/Vector3 linear
  float64 x
  float64 y
  float64 z
geometry_msgs/Vector3 angular
  float64 x
  float64 y
  float64 z

详情请查看补充资料。

2.实现发布节点

/*
    编写 ROS 节点，控制小乌龟画圆
    准备工作:
        1.获取topic(已知: /turtle1/cmd_vel)
        2.获取消息类型(已知: geometry_msgs/Twist)
        3.运行前，注意先启动 turtlesim_node 节点
    实现流程:
        1.包含头文件
        2.初始化 ROS 节点
        3.创建发布者对象
        4.循环发布运动控制消息
*/
#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"control");
    ros::NodeHandle nh;
    // 3.创建发布者对象 <模板泛型>（“节点名称（与官方的保持一致，可topic list查看）”，最大缓存队列长度数量）
    ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel",1000);
    // 4.循环发布运动控制消息
    //4-1.组织消息
    geometry_msgs::Twist msg;  //实例化消息对象  格式与官方保持一致（可以 rosmsg info 该消息   查看各式）
    msg.linear.x = 1.0;
    msg.linear.y = 0.0;
    msg.linear.z = 0.0;
    msg.angular.x = 0.0;
    msg.angular.y = 0.0;
    msg.angular.z = 2.0;
    //4-2.设置发送频率
    ros::Rate r(10);
    //4-3.循环发送
    while (ros::ok())
    {
        pub.publish(msg);
        ros::spinOnce();
    }
    return 0;
}

实现方案B: Python

#! /usr/bin/env python
"""
    编写 ROS 节点，控制小乌龟画圆
    准备工作:
        1.获取topic(已知: /turtle1/cmd_vel)
        2.获取消息类型(已知: geometry_msgs/Twist)
        3.运行前，注意先启动 turtlesim_node 节点
    实现流程:
        1.导包
        2.初始化 ROS 节点
        3.创建发布者对象
        4.循环发布运动控制消息
"""
import rospy
from geometry_msgs.msg import Twist
if __name__ == "__main__":
    # 2.初始化 ROS 节点
    rospy.init_node("control_circle_p")
    # 3.创建发布者对象
    pub = rospy.Publisher("/turtle1/cmd_vel",Twist,queue_size=1000)
    # 4.循环发布运动控制消息
    rate = rospy.Rate(10)
    msg = Twist()
    msg.linear.x = 1.0
    msg.linear.y = 0.0
    msg.linear.z = 0.0
    msg.angular.x = 0.0
    msg.angular.y = 0.0
    msg.angular.z = 0.5
    while not rospy.is_shutdown():
        pub.publish(msg)
        rate.sleep()

3.运行

2、实操——话题（topic）订阅(sub)

需求描述:** **已知turtlesim中的乌龟显示节点，会发布当前乌龟的位姿(窗体中乌龟的坐标以及朝向)，要求控制乌龟运动，并时时打印当前乌龟的位姿。

结果演示:

实现分析:

1. 首先，需要启动乌龟显示以及运动控制节点并控制乌龟运动。
2. 要通过ROS命令，来获取乌龟位姿发布的话题以及消息。
3. 编写订阅节点，订阅并打印乌龟的位姿。

实现流程:

1. 通过ros命令获取话题与消息信息。
2. 编码实现位姿获取节点。
3. 启动 roscore、turtlesim_node 、控制节点以及位姿订阅节点，控制乌龟运动并输出乌龟的位姿。

1.话题与消息获取

获取话题:/turtle1/pose

rostopic list

获取消息类型:turtlesim/Pose

rostopic type  /turtle1/pose

获取消息格式:

rosmsg info turtlesim/Pose

响应结果:

float32 x
float32 y
float32 theta
float32 linear_velocity
float32 angular_velocity

2.实现订阅节点

实现方案A: C++

/*  
    订阅小乌龟的位姿: 时时获取小乌龟在窗体中的坐标并打印
    准备工作:
        1.获取话题名称 /turtle1/pose
        2.获取消息类型 turtlesim/Pose
        3.运行前启动 turtlesim_node 与 turtle_teleop_key 节点
    实现流程:
        1.包含头文件
        2.初始化 ROS 节点
        3.创建 ROS 句柄
        4.创建订阅者对象
        5.回调函数处理订阅的数据
        6.spin
*/
#include "ros/ros.h"
#include "turtlesim/Pose.h"
//ConstPtr&常量指针
void doPose(const turtlesim::Pose::ConstPtr& p){
    ROS_INFO("乌龟位姿信息:x=%.2f,y=%.2f,theta=%.2f,lv=%.2f,av=%.2f",
        p->x,p->y,p->theta,p->linear_velocity,p->angular_velocity
    );
}
int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"sub_pose");
    // 3.创建 ROS 句柄
    ros::NodeHandle nh;
    // 4.创建订阅者对象
    ros::Subscriber sub = nh.subscribe<turtlesim::Pose>("/turtle1/pose",1000,doPose);
    // 5.回调函数处理订阅的数据
    // 6.spin
    ros::spin();
    return 0;
}