ROS学习笔记03、ROS通信机制（话题通信、服务通信、参数服务器）

前言

马上开学，目前学校很多实验室都是人工智能这块，大部分都是和机器人相关，然后软件这块就是和cv、ros相关，就打算开始学习一下。

本章节是虚拟机安装Ubuntu18.04以及安装ROS的环境。

学习教程：【Autolabor初级教程】ROS机器人入门，博客中一些知识点是来源于赵老师的笔记在线笔记，本博客主要是做归纳总结，如有侵权请联系删除。

视频中的案例都基本敲了遍，这里给出我自己的源代码文件：

链接：https://pan.baidu.com/s/13CAzXk0vAWuBsc4oABC-_g 
提取码：0hws 

所有博客文件目录索引：博客目录索引(持续更新)

一、话题通信

原理

模型涉及到：ROS Master (管理者)、Talker (发布者)、Listener (订阅者)。

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

0、Talker（发布者）启动后，首先会进行RPC来向ROS Master注册自身信息，其中就包含发布的话题名称，此时ROS Master也会将节点的注册信息加入到注册表。

1、Listener（订阅者）启动后，也会进行RPC来向ROS Master注册自身信息，其中包含需要订阅的话题名，此时ROS Master也会将节点的注册信息加入到注册表。

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

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

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

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

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

重点：

1、前五步是RPC连接，最后两步是TCP协议进行数据传输。

2、Talker 与 Listener 的启动无先后顺序要求。

3、Talker 与 Listener 都可以有多个。

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

1.1、实现发布订阅

# 进入到工程下的src目录
cd /home/workspace/roslearn/src

# 创建名为plumbing_pub_sub的包
catkin_create_pkg --rosdistro melodic plumbing_pub_sub roscpp rospy std_msgs

c++代码实现

发布者

demo01_pub.cpp：发布话题chatter，并且不断的向话题chatter发送消息

流程：初始化节点，实例化ros句柄，根据句柄来创建发布者对象（指明发布消息），接着使用发布者对象来进行发送消息。

#include "ros/ros.h"  //引入ros文件
#include "std_msgs/String.h"  //普通文本信息
#include <sstream>  //之后可以创建stringstream对象来进行拼接

int main(int argc, char *argv[]) {

    //防止日志出现乱码
    setlocale(LC_CTYPE, "zh_CN.utf8");
    setlocale(LC_ALL, "");

    //2、初始化节点命名
    //参数1、2后期节点传值使用
    //参数3是节点名称，标识符在ROS网路拓扑中唯一
    ros::init(argc, argv, "changlu_pub");
    //3、实例化ROS句柄
    ros::NodeHandle nh;//该类封装了ROS的一些常用功能

    //4、实例化  发布者 对象
    //泛型：发布的消息类型
    //参数1：发布的话题
    //参数2：队列中最大保存的消息数，超过阈值，先到的先销毁（满的时候先来先销毁）
    ros::Publisher pub = nh.advertise<std_msgs::String>("chatter", 10);

    //解决订阅方消息丢失问题
    //ros::Duration(3.0).sleep();//睡眠3秒

    //5、组织被发布的数据，编写逻辑发布数据
    std_msgs::String msg;

    //要求按照10hz的频率来发布消息
    ros::Rate rate(10);

    int count = 0;

    //死循环，节点活着就一直运行
    while (ros::ok()) {
        count++;
        //借助stringstream来拼接数据
        std::stringstream ss;
        ss << "hello ----> " << count; //拼接字符串
        //将string流中的数据赋值给msg对象
        msg.data = ss.str();

        //发布消息
        pub.publish(msg);
        ROS_INFO("向主题【chatter】发布数据：%s", ss.str().c_str());

        //进行阻塞睡眠
        rate.sleep();
    }

    return 0;
}

编辑CMakeLists.txt：

add_executable(demo01_pub src/demo01_pub.cpp)

target_link_libraries(demo01_pub
  ${catkin_LIBRARIES}
)

编写好之后首先对整个项目进行编译（可使用vscode快捷键前提需要配置）！

测试：

窗口一：执行roscore

窗口二：执行发布者代码，就是创建

# 刷新环境变量
source ./devel/setup.bash
   
# 执行发布者节点
rosrun plumbing_pub_sub demo01_pub

窗口三：检验是否有消息发布

# 订阅话题chatter并打印
rostopic echo chatter

此时表示成功发送：

订阅者

demo01_sub.cpp：不断的从主题chatter接收消息并进行日志打印

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

void doMsg(const std_msgs::String::ConstPtr& msg_p) {
    ROS_INFO("订阅者1号订阅到【chatter】信息：%s", msg_p->data.c_str());
}

int main(int argc, char *argv[]) {
    //设置编码（防止乱码问题）
    setlocale(LC_ALL, "");

    //初始化节点
    ros::init(argc, argv, "changlu_sub");

    //实例化句柄
    ros::NodeHandle nh;

    //使用句柄创建订阅者
    ros::Subscriber sub = nh.subscribe<std_msgs::String>("chatter", 10, doMsg);

    //使用spin()来执行回调队列中的回调函数
    ros::spin();

    return 0;

}

修改CMakeLists.txt：新增以下内容

add_executable(demo01_sub src/demo01_sub.cpp)

target_link_libraries(demo01_sub
  ${catkin_LIBRARIES}
)

之后就是重新编译整个项目，然后来开始测试发布方与订阅方：

第一个窗口：执行roscore

第二个窗口：启动发布者，与之前一样。

第三个窗口：代替之前手动命令的rostopic，而是去启动我们自己定义的订阅者程序。

# 刷新环境变量
source ./devel/setup.bash
   
# 执行订阅者节点
rosrun plumbing_pub_sub demo01_sub

解决消息丢失问题

若是我们先启动订阅者，再启动发布者，可以很明显的看到有部分的消息丢失了，如下图所示：

原因描述：在发布者的代码中，创建发布者的代码并不是同步操作，也就是说真正创建好发布者实例时，已经下面的while执行了三四条，所以实际上压根就没有发出去那几条看似消失了的消息。

ros::Publisher pub = nh.advertise<std_msgs::String>("chatter", 10);

解决方案：

就是让我们的发布者实例创建完成后向下执行发布操作就行了，所以我们这里去让其睡眠3秒，在这段时间中肯定是能够创建成功的：

ros::Duration(3.0).sleep();//睡眠3秒

再次尝试下：成功解决！

使用计算图来查看发布订阅者的关系

在启动生产者与订阅者之后，我们来使用计算图来查看一下它们的关系：

rqt_graph

• /changlu_pub：发布者。
• /chatter：主题名。
• /changlu_sub：订阅者。

---

python代码实现

实际上就是复刻了一遍c++的代码实现

发布者

demo01_pub_p.py：每一秒向主题chatter发送一条消息

#! /usr/bin/env python
# coding=utf-8

import rospy
from std_msgs.msg import String

if __name__ == "__main__":
    # 1、初始化节点
    rospy.init_node("talker_p");

    # 2、实例化发布者对象
    pub = rospy.Publisher("chatter", String, queue_size=10);

    # 解决消息丢失问题，睡眠一秒
    rospy.sleep(1.)

    # 创建msg对象
    msg = String()
    msg_front = "hello i from pub_p"

    # 计数器
    count = 0
    # 设置循环频率
    rate = rospy.Rate(1)

    # 循环发送
    while not rospy.is_shutdown():
        # 3、发布消息
        msg.data = msg_front + str(count)  # 发送消息拼接
        pub.publish(msg)
        rate.sleep()
        rospy.loginfo("发布到主题【chatter】：%s", msg.data)
        count += 1

订阅者

demo01_sub_p.py：不断接收主题chatter的消息

#! /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__":
    # 1、初始化节点
    rospy.init_node("listen_p")
    
    # 2、实例化订阅者
    rospy.Subscriber("chatter", String, doMsg, queue_size=10)

    # 3、处理订阅队列中的消息
    rospy.spin()

接着去编辑cmakelists：

# 配置好对应scripts目录下的python文件
catkin_install_python(PROGRAMS
  scripts/demo01_pub_p.py
  scripts/demo01_sub_p.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

给scripts目录下的python文件添加可执行权限：

cd /home/workspace/roslearn/src/plumbing_pub_sub/scripts

chmod +x demo01_pub_p.py demo01_sub_p.py

接着去编译整个项目（在工程目录下执行catkin_make），来对python版本的发布者与订阅者进行校验：

窗口一：执行roscore命令

窗口二：执行python版的发布者程序

source ./devel/setup.bash

rosrun plumbing_pub_sub demo01_pub_p.py

窗口三：执行python版的订阅者程序

source ./devel/setup.bash

rosrun plumbing_pub_sub demo01_sub_p.py

接着来使用计算图命令来查看我们的节点信息：

rqt_graph

c++与python节点通信

在ros中，无论是c++编写的还是python编写的发布者与订阅者都可以是进行相互通信的，下面来进行尝试下：

发布者为python写的demo，订阅者是c++写的demo：

看下发布者信息：

消费者接收的消息：成功消费

1.2、实现发布订阅自定义消息

自定义msg

步骤一：定义消息结构体在msg目录下

定义消息结构体：Person.msg

string name
uint16 age
float64 height

步骤二：对配置文件package.xml、CMakeLists.txt进行配置修改

在pakage.xml中添加两行配置：

<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>

CMakeLists.txt：

# 添加message_generation
find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  message_generation
)

# 指向消息目录以及对应的消息体
## Generate messages in the 'msg' folder
add_message_files(
  FILES
  Person.msg
)

# 生成消息时依赖于 std_msgs
## Generate added messages and services with any dependencies listed here
generate_messages(
  DEPENDENCIES
  std_msgs
)

# 执行时依赖需要message_runtime
catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES plumbing_pub_sub
  CATKIN_DEPENDS roscpp rospy std_msgs message_runtime
#  DEPENDS system_lib
)

接着我们来对整个项目进行编译，ros就会给我们生成对应c++的消息结构体源代码以及python的消息结构体源代码：

之后使用某种语言来去创建指定msg对象就是在这里去进行调用的。

---

C++代码实现

配置自定义msg依赖

对于我们之前自定义的msg，若是不在vscode中进行额外配置路径，在下方进行编码代码的时候，对于自定义消息对象类型我们就无法使用快捷键提示了！

"/home/workspace/roslearn/devel/include/**"

发布者

demo02_pub_person.cpp：编写发布者代码，主题为chatter_person，发送自定义的消息数据。

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

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

    //1、初始化节点
    ros::init(argc, argv, "talker_person");

    //2、初始化ros句柄
    ros::NodeHandle nh;

    //3、根据句柄来实例化发布者对象
    ros::Publisher pub = nh.advertise<plumbing_pub_sub::Person>("chatter_person", 1000);

    ros::Duration(2).sleep();

    //4、组织发布消息
    plumbing_pub_sub::Person p;
    p.name = "changlu";
    p.age = 2020;
    p.height = 1.45;

    ros::Rate r(1);

    //5、循环发送数据
    while (ros::ok()) {
        //发布数据
        pub.publish(p);
        
        //日志打印
        ROS_INFO("发布者向主题【chatter_person】发送消息 => name:%s, age:%d, height:%.2f",p.name.c_str(), p.age, p.height);
        
        r.sleep();
        ros::spinOnce();

        p.age += 1;
    }
    return 0;
}

配置CMakeLists.txt：下面的配置项顺序不能有误，否则会报异常

add_executable(demo02_pub_person src/demo02_pub_person.cpp)

# 额外多了配置这个依赖，目的就是在编译demo02_pub_person需要去先编译对应的自定义msg结构体，否则会报错
# demo02_pub_person指的就是上面对应cpp文件的映射名，后者就是自定义生成message消息的c文件，指明当前的工程名plumbing_pub_sub
add_dependencies(demo02_pub_person ${PROJECT_NAME}_generate_messages_cpp)

target_link_libraries(demo02_pub_person
  ${catkin_LIBRARIES}
)

编译整个项目，接着我们就可以去启动发布者并进行测试了：

窗口一：执行roscore命令，来启动ros核心服务。

窗口二：执行发布者c++程序，向主题发布消息

# 进入到工程目录下也就是roslearn目录
source ./devel/setup.bash

rosrun plumbing_pub_sub demo02_pub_person

窗口三：执行rostopic命令来进行接收消息

# 订阅主题为：chatter_person
rostopic echo chatter_person

订阅者

demo02_sub_person.cpp：

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

void doPerson(const plumbing_pub_sub::Person::ConstPtr& person_p) {
    ROS_INFO("订阅到主题【chatter_person】消息为：%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::init(argc, argv, "listener_person");

    //2、实例化句柄
    ros::NodeHandle nh;

    //3、使用句柄来实例化订阅者
    ros::Subscriber sub = nh.subscribe<plumbing_pub_sub::Person>("chatter_person", 10, doPerson);

    //4、执行回调函数
    ros::spin();

    return 0;
}

配置CMakeLists.txt：

add_executable(demo02_sub_person src/demo02_sub_person.cpp)

add_dependencies(demo02_sub_person ${PROJECT_NAME}_generate_messages_cpp)

target_link_libraries(demo02_sub_person
  ${catkin_LIBRARIES}
)

接着我们就可以编译项目，然后去执行服务：

窗口一：执行roscore服务

窗口二：执行订阅者服务

source ./devel/setup.bash

rosrun plumbing_pub_sub demo02_sub_person

等待发布者服务执行，发布者服务启动后，就可以接收到消息了！

窗口三：执行发布者服务

rosrun plumbing_pub_sub demo02_pub_person

二、服务通信

原理

三个角色：ROS master(管理者)、Server(服务端)、Client(客户端)

ROS Master 负责保管 Server 和 Client 注册的信息，并匹配话题相同的 Server 与 Client ，帮助 Server 与 Client 建立连接，连接建立后，Client 发送请求信息，Server 返回响应信息。

1、server启动，使用rpc向ros master注册，master会将节点信息注册到注册表中。

2、client启动，使用rpc向ros master注册，master会将节点信息注册到注册表中。

3、ros master根据注册表中匹配client、server，通过rpc向client发送server的TCP地址信息。

4、client与服务节点使用TCP来建立网络连接。

5、server接收并处理请求，响应结果给client。

2.1、创建服务通信功能包

# 进入工程目录中的src目录下
cd /home/workspace/roslearn/src

# 创建功能包名为plumbing_server_client
catkin_create_pkg --rosdistro melodic plumbing_server_client roscpp rospy std_msgs

---

2.2、自定义rsv（request、response规范）

1、编写srv文件（规范某个请求的request、response）

在新创建ros的包下定义srv文件夹，创建自定义请求srv文件：

# 客户端定义请求结构
int32 num1
int32 num2
---
# 服务端响应结构
int32 sum

2、编写配置文件

package.xml配置编译依赖：

<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>

CMakeLists.txt配置srv相关配置：

# 添加message_generation
find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  message_generation
)

# 添加自定义的srv文件
add_service_files(
  FILES
  AddInts.srv
)

# 编译消息
generate_messages(
  DEPENDENCIES
  std_msgs
)

3、最终我们来对整个工程进行编译

ok，若是编译没有报错，我们此时就可以去查看是否生成了c++版本的request、response以及python版本的：

2.3、C++代码实现

服务方

提供服务：计算请求参数num1、num2的和，并响应结果。

demo01_server.cpp：

#include "ros/ros.h"
#include "plumbing_server_client/AddInts.h"

//处理请求函数
bool doReq(plumbing_server_client::AddInts::Request& req,
            plumbing_server_client::AddInts::Response& resp) {
    int num1 = req.num1;
    int num2 = req.num2;
    //打印日志信息
    ROS_INFO("服务器收到请求信息：num1 = %d, num2 = %d", num1, num2);
    //逻辑处理
    if (num1 < 0 || num2 < 0) {
        ROS_ERROR("提交的数据异常：num1与num2不可为负数！");
        return false;
    }
    resp.sum = num1 + num2;
    ROS_INFO("结果求值sum=%d", resp.sum);
    return true;
}

int main(int argc, char *argv[]){

    setlocale(LC_ALL, "");

    //1、初始化节点
    ros::init(argc, argv, "AddInts_Server");

    //2、初始化句柄
    ros::NodeHandle nh;

    //3、句柄初始化服务
    ros::ServiceServer server = nh.advertiseService("AddInts", doReq);
    ROS_INFO("节点【AddInts_Server】已启动！");

    //4、回调函数处理请求
    ros::spin();
    return 0;
}

配置CMakeLists.txt：

add_executable(AddInts_Server src/demo01_server.cpp)

add_dependencies(AddInts_Server ${PROJECT_NAME}_gencpp)

target_link_libraries(AddInts_Server
  ${catkin_LIBRARIES}
)

接下来进行编译项目，然后就可以运行服务并测试了：

第一个窗口：执行命令roscore，启动ros的核心服务。

第二个窗口：启动服务。

source ./devel/setup.bash

rosrun plumbing_server_client AddInts_Server

第三个窗口：使用rosservice命令行工具来进行测试

# 查看服务列表
rosservice list

# 发起请求，参数为12，13
rosservice call AddInts 12 13

# 编写到AddInts时就可以按tab，来对参数进行自动填充
rosservice call AddInts "num1: -1
num2: -1"

看下服务端是否收到请求：

客户端（含优化）

demo01_client.cpp：

#include "ros/ros.h"
#include "plumbing_server_client/AddInts.h"

//默认自带了1个参数
int main(int argc, char *argv[])
{
    setlocale(LC_ALL, "");

    //1、初始化节点
    ros::init(argc, argv, "AddInts_Client");

    //2、创建Ros句柄
    ros::NodeHandle nh;

    //3、创建客户端实例对象
    ros::ServiceClient client = nh.serviceClient<plumbing_server_client::AddInts>("AddInts");

    //4、实例化请求对象
    plumbing_server_client::AddInts ai;
    ai.request.num1 = 1;
    ai.request.num2 = 2;

    //5、发送请求对象
    bool flag = client.call(ai);
    if (flag) {
        //请求成功
        ROS_INFO("请求正常处理，响应结果为：%d", ai.response.sum);
    }else {
        ROS_INFO("请求处理失败！");
        return 1;
    }

    return 0;
}

修改配置文件CMakeLists.txt：

add_executable(AddInts_Client src/demo01_client.cpp)

add_dependencies(AddInts_Client ${PROJECT_NAME}_gencpp)

target_link_libraries(AddInts_Client
  ${catkin_LIBRARIES}
)

编译整个项目文件， 来进行测试服务

窗口一：执行roscore，启动ros服务。

窗口二：启动当前的服务节点，用于计算num1+num2。

source ./devel/setup.bash

rosrun plumbing_server_client AddInts_Server

窗口三：发起请求，取得结果数据。

source ./devel/setup.bash 

rosrun plumbing_server_client AddInts_Client

看一下服务端处理内容：

优化

优化1：根据命令行携带参数来动态更改请求数据，发起请求。

//优化点1：根据命令行携带参数来动态更改请求数据，发起请求。
//保证当前命令种包含两个参数
if (argc != 3) {
    ROS_INFO("当前命令参数数量不符合要求！");
    return 1;
}

//优化1：动态获取参数。atoi表示字符串转换成整型数
ai.request.num1 = atoi(argv[1]);
ai.request.num2 = atoi(argv[2]);

搭配命令：

# 后面1 2就是携带的参数
rosrun plumbing_server_client AddInts_Client 1 2

优化2：若是客户端先执行，服务端后执行，解决客户端直接失败情况，可延迟等待服务节点在线后发起请求调用。

若是服务端先不启动，先执行客户端，那么此时就会直接报服务调用失败，如下：

如何优化呢？ros中给我们提供了一个阻塞等待服务的一个接口，我们只需要在call方法前调用即可：

//优化点2：解决服务当前不在线的情况
//若是当前不在线，会进入到阻塞状态
client.waitForExistence();

三、参数服务器

原理

介绍

多个节点共享数据（基于内存）。

场景：导航实现时，进行路径规划，①全局路径优化：设计一个从出发点到目的点的大致路径。②本地路径优化：根据当前路况生成时的行进路径。

使用：可供节点继续增删改查。

原理

talker与listener使用rpc来进行调用。

参数可使用数据类型:

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

---

3.1、创建参数服务器功能包

# 进入工程目录中的src目录下
cd /home/workspace/roslearn/src

# 创建功能包名为plumbing_param_server
catkin_create_pkg --rosdistro melodic plumbing_param_server roscpp rospy std_msgs

3.2、实现增删改查代码

两种方式：①ros::NodeHandle。②ros::param。

demo01_param_set.cpp：包含增删改查的实际代码

#include "ros/ros.h"

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

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

    //增--------------------
    //方案1：nh
    nh.setParam("type", "changlu");
    nh.setParam("radius", 0.15);
    //方案2：ros::param
    ros::param::set("type_param", "changluchanglu");
    ros::param::set("radius_param", 0.15);

    //查--------------------
    ROS_INFO("====开始查询===");
    //1、param(键,默认值) ：存在，返回对应结果，否则返回默认值
    double radius = nh.param("radius1", 0.5);
    ROS_INFO("查询key为【radius1】，其值为：%.2f", radius);//理想：0.5
    //2、getParam(键,存储结果的变量)：存在,返回 true,且将值赋值给参数2；若果键不存在，那么返回值为 false，且不为参数2赋值。
    radius = 0.0;
    bool flag = nh.getParam("radius", radius);//理想是查询成功，0.15
    if (flag) {
        ROS_INFO("查询key为【radius】成功，值为：%.2f", radius);
    }else {
        ROS_INFO("查询key为【radius】失败！");
    }
    //3、getParamCached，与getParam的效果相同。首先查看当前缓存是否有该param，若是有直接取出，没有再从远端查询出来。
    radius = 0.0;
    flag = nh.getParamCached("radius", radius);
    if (flag) {
        ROS_INFO("查询cached key为【radius】成功，值为：%.2f", radius);
    }else {
        ROS_INFO("查询cached key为【radius】失败！");
    }

    //4、getParamNames(std::vector<std::string>)：获取所有的键,并存储在参数 vector 中 。
    std::vector<std::string> names;//集合元素
    nh.getParamNames(names);
    ROS_INFO("遍历所有的键：");
    for (auto &&name: names) {
        ROS_INFO("%s", name.c_str());
    }

    //5、hasParam(键)：是否包含某个键，存在返回 true，否则返回 false。
    flag = nh.hasParam("radius");
    if (flag) {
        ROS_INFO("键为【radius】存在！");
    }else {
        ROS_INFO("键为【radius】不存在！");
    }

    //6、searchParam(参数1，参数2)：搜索键，参数1是被搜索的键，参数2存储搜索结果的变量，就是/键；若是不存在，则返回的就是空串。
    std::string key;
    nh.searchParam("radius", key);
    ROS_INFO("searchParam搜索键为【radius】得到的key为：%s", key.c_str());//理想：/radius

    //ros::param::xx同样也封装了与nh类似的查询代码（同上）

    //改--------------------
    nh.setParam("type", "changlu123");
    ros::param::set("radius", 0.3);

    //删--------------------（若是存在则删除成功，若是不存在则删除失败！）
    //方式一：ros::NodeHandle，deleteParam("键")：根据键删除参数，删除成功，返回 true，否则(参数不存在)，返回 false。
    flag = nh.deleteParam("radius");
    if (flag) {
        ROS_INFO("ros::NodeHandle删除键【radius】成功！");
    }else {
        ROS_INFO("ros::NodeHandle删除键【radius】失败！");
    }

    //方式二：ros::param，del("键")：根据键删除参数，删除成功，返回 true，否则(参数不存在)，返回 false。
    flag = ros::param::del("radius");
    if (flag) {
        ROS_INFO("ros::param删除键【radius】成功！");
    }else {
        ROS_INFO("ros::param删除键【radius】失败！");
    }
    return 0;
}

编辑CMakeLists.txt：

add_executable(demo01_param_set src/demo01_param.cpp)

target_link_libraries(demo01_param_set
  ${catkin_LIBRARIES}
)

编译整个项目，来进行测试：

rosparam指令

对应的rosparam指令：

rosparam list

rosparam get /type

rosparam get /radius

rosparam get /type_param

rosparam get /radius_param

四、常用命令

ros-wiki的详细命令工具

rosnode

rosnode 是用于获取节点信息的命令。

rosnode ping    测试到节点的连接状态    # 示例：rosnode ping /changlu_pub
rosnode list    列出活动节点           # 示例：rosnode list
rosnode info    打印节点信息           # 示例：rosnode info /changlu_pub
rosnode machine    列出指定设备上节点   # 示例：rosnode machine changlu-VirtualBox  # 对应的是当前主机设备名称
rosnode kill    杀死某个节点           # 示例：rosnode kill /changlu_pub
rosnode cleanup    清除不可连接的节点   # 示例（针对于kill没有杀死的节点，僵尸节点）：rosnode cleanup /changlu_pub

rostopic

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

rostopic bw     显示主题使用的带宽          # 示例：rostopic bw /chatter
rostopic delay  显示带有 header 的主题延迟  # 示例：rostopic delay /chatter。若是没有head就会报error
rostopic echo   打印消息到屏幕             # 示例：rostopic echo /chatter
rostopic find   根据消息类型查找主题           # 示例：rostopic find plumbing_pub_sub/Person
rostopic hz     显示主题的发布频率         # 示例：rostopic hz chatter
rostopic info   显示主题相关信息，包含发布者，订阅者   # 示例：rostopic info chatter 
rostopic list   显示所有活动状态下的主题      # 示例：rostopic list
rostopic pub    将数据发布到主题            # 示例：rostopic pub chatter plumbing_pub_sub/Person "name: ''
# age: 0
# height: 0.0"
rostopic type   打印主题类型               # 示例：rostopic type chatter

rosmsg

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

rosmsg show    显示消息描述结构体          # 示例：rosmsg show plumbing_pub_sub/Person
rosmsg info    显示消息信息               # 示例：rosmsg info plumbing_pub_sub/Person
rosmsg list    列出所有消息               # 示例：rosmsg list
rosmsg md5    显示 md5 加密后的消息        # 示例：rosmsg md5 plumbing_pub_sub/Person
rosmsg package    显示某个功能包下的所有消息 # 示例：rosmsg package plumbing_pub_sub
rosmsg packages    列出包含消息的功能包     # 示例：rosmsg packages plumbing_pub_sub

rosservice

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

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

rosservice args     打印服务参数           # 示例：rosservice args /AddInts
rosservice call    使用提供的参数调用服务   # 示例：rosservice call /AddInts "num1: 0
# num2: 0" 
# sum: 0
rosservice find    按照消息类型查找服务      # 示例：rosservice find plumbing_server_client/AddInts
rosservice info    打印有关服务的信息        # 示例：rosservice info /AddInts
rosservice list    列出所有活动的服务        # 示例：rosservice list
rosservice type    根据指定服务打印服务类型   # 示例：rosservice type /AddInts
rosservice uri    打印服务的 ROSRPC uri    # 示例：rosservice uri /AddInts

rossrv

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

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

rosparam

rosparam包含rosparam命令行工具，用于使用YAML编码文件在参数服务器上获取和设置ROS参数。

rosparam set    设置参数             # 示例：rosparam set name changlu
rosparam get    获取参数             # 示例：rosparam get /name
rosparam load    从外部文件加载参数    # 示例：rosparam load /home/workspace/roslearn/param.yaml
rosparam dump    将参数写出到外部文件  # 示例：rosparam dump /home/workspace/roslearn/param.yaml
rosparam delete    删除参数         # 示例：rosparam delete sex
rosparam list    列出所有参数        # 示例：rosparam list

五、通信机制实战

实战内容

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

目的：熟悉、强化通信模式应用。

创建通信机制实战工具包

# 进入工程目录中的src目录下
cd /home/workspace/roslearn/src

# 创建功能包名为communication_practice
# 新增一个geometry_msgs
catkin_create_pkg --rosdistro melodic communication_practice roscpp rospy std_msgs geometry_msgs

5.1、话题发布实战

目标

目标说明

在话题发布实战中，我们需要去自己去编写一个话题发布者服务来向指定的话题发布消息，以此来控制turtleslm这个订阅节点服务在GUI界面上乌龟的移动。

首先我们将roscore、gui以及键盘控制节点都进行启动：

# 启动系统自带的节点，实现通信
roscore

# 弹出图像化界面
rosrun turtlesim turtlesim_node

# 读取键盘上下左右移动信息控制乌龟行动
rosrun turtlesim turtle_teleop_key

确定主题及消息格式

在启动上面节点之后，我们就可以使用计算图来查看一下节点与结点之间关系：

rqt_graph

从图上我们就能够得知两个结点名称以及对应的主题：

我们来查看当前所有的主题：rostopic list

接着获取/turtle1/cmd_vel的消息类型：geometry_msgs/Twist

rostopic type /turtle1/cmd_vel

在获取geometry_msgs/Twist的消息格式：

rosmsg info geometry_msgs/Twist

• 对于线速度与角速度讲解：【Autolabor初级教程】ROS机器人入门-线速度与角速度
• 关于弧度与角速度的知识点与图示：2.5.1 实操01_话题发布-弧度与角速度示例图

# linear(线速度) 下的xyz分别对应在x、y和z方向上的速度(单位是 m/s)：前进或后退的速度
# x前后  y左右  z上下
geometry_msgs/Vector3 linear
  float64 x
  float64 y
  float64 z
  
# angular(角速度)下的xyz分别对应x轴上的翻滚、y轴上俯仰和z轴上偏航的速度(单位是rad/s)：转弯的速度，弧度每秒
# x翻滚(二维下没有) y俯仰(抬头低头动作，二维没有)  z偏航(左右转，二维可以有)
geometry_msgs/Vector3 angular
  float64 x
  float64 y
  float64 z

来看下对应的msg消息文件的路径及内容：

此时我们来利用命令行来实现向该turtle1主题去发布消息：

# 每秒钟10次向主题【/turtle1/cmd_vel】发送geometry_msgs/Twist格式的消息，订阅者也就是乌龟会对其进行消费移动
# 1Hz代表每秒钟震动1次,60Hz代表每秒震动60次
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: 1.0"

实际上当前我们就已经实现了控制乌龟旋转的功能了，但是对于一些更为辅助的情况时，我们就需要自己编写一个发布者节点来控制乌龟的方向：

我们也可以使用ros工具来查看当前订阅主题的消息：

rostopic echo /turtle1/cmd_vel

---

实战话题发布控制乌龟的行进（C++）

turtle_pub.cpp：

#include "ros/ros.h"
#include "geometry_msgs/Twist.h"

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

    //1、初始化ros节点
    ros::init(argc, argv, "control");

    //2、初始化句柄
    ros::NodeHandle nh;

    //3、使用句柄初始化发布者对象
    ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 1000);
    ros::Duration(3).sleep();
    ROS_INFO("发布者服务已启动！");

    //4、循环发布指定消息
    geometry_msgs::Twist msg;
    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 = 1.0;

    ros::Rate r(10);
    while (ros::ok()) {
        pub.publish(msg);
        ROS_INFO("消息已发送");
        ros::spinOnce();
    }
    return 0;
}

修改配置文件CMakeLists.txt：

add_executable(turtle_control src/turtle_pub.cpp)

target_link_libraries(turtle_control
  ${catkin_LIBRARIES}
)

重新编译整个项目，来进行启动服务：

source ./devel/setup.bash

rosrun communication_practice turtle_control

---

5.2、话题订阅实战

目标

目标说明

通过主题来订阅获取小乌龟的位姿信息，首先使用命令行，接着来进行代码编写实操。

确认乌龟位姿主题及消息格式

查询当前所有的话题列表：

rostopic list

获取乌龟位姿话题的消息类型：

rostopic type /turtle1/pose

获取对应位姿的消息格式：

rosmsg info turtlesim/Pose

float32 x
float32 y
float32 theta
float32 linear_velocity
float32 angular_velocity

启动ros命令来使用订阅者去订阅主题【/turtle1/pose】：

rostopic echo /turtle1/pose

---

实战话题订阅乌龟的位姿信息（C++）

turtle_sub_pose.cpp：

#include "ros/ros.h"
#include "turtlesim/Pose.h"

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, "");
    //初始化节点
    ros::init(argc, argv, "pose_sub");

    //初始化句柄
    ros::NodeHandle nh;

    //根据句柄初始化订阅者
    ros::Subscriber sub = nh.subscribe<turtlesim::Pose>("/turtle1/pose", 1000, doPose);
    ROS_INFO("订阅者已实例化！");

    ros::spin();
    
    return 0;
}

配置文件添加配置项：

add_executable(turtle_sub_pose src/turtle_sub_pose.cpp)

target_link_libraries(turtle_sub_pose
  ${catkin_LIBRARIES}
)

编译项目首先启动ros核心master及乌龟gui图：

start_turtle.launch.launch： 启动注释中的两行命令

<launch>
    <!-- 
        source ./devel/setup.bash
        roslaunch communication_practice start_turtle.launch.launch
    -->
    <!-- GUI图像界面 -->
    <node pkg="turtlesim" type="turtlesim_node" name="turtle_GUI"/>
    <!-- 控制台界面 -->
    <node pkg="turtlesim" type="turtle_teleop_key" name="turtle_KEY" />
</launch>

接着来启动订阅节点：

source ./devel/setup.bash

rosrun communication_practice turtle_sub_pose

---

5.3、服务调用实战

目标

目标说明

通过编码来向turtlesim发送请求，在乌龟显示节点的窗体指定位置生成一乌龟。

确认turtlesim服务及消息类型、格式

查询所有服务列表，确定turtlesim提供的服务：/spawn

rosservice list

查看服务的消息类型：turtlesim/Spawn

rosservice type /spawn

获取消息格式：

rossrv info turtlesim/Spawn

格式如下：

float32 x
float32 y
float32 theta
string name
---
string name

• x,y为坐标，theta就是朝向(一圈是3.14)，name则是乌龟名称。

我们也可以直接通过ros命令来生成出乌龟：

rosservice call /spawn "x: 1.0
y: 4.0
theta: 1.5
name: 'turtle3'" 
name: "turtle3"

实战服务调用生成乌龟（C++）

turtle_client_set.cpp：

#include "ros/ros.h"
#include "turtlesim/Spawn.h"

int main(int argc, char *argv[])
{
    setlocale(LC_ALL, "");
    //1、初始化节点
    ros::init(argc,  argv, "setTurtle_node");

    //2、实例化句柄
    ros::NodeHandle nh;

    //3、使用句柄实例化服务客户端
    ros::ServiceClient client = nh.serviceClient<turtlesim::Spawn>("/spawn");

    //4、等待服务启动
    //方式一：使用客户端实例的方法
    // client.waitForExistence();
    //方式二：使用ros提供的方法
    ros::service::waitForService("/spawn");

    //5、发送请求
    //填充request请求对象
    turtlesim::Spawn spawn;
    spawn.request.x = 1;
    spawn.request.y = 1;
    spawn.request.theta = 1.5;
    spawn.request.name = "turtle_No2";

    bool flag = client.call(spawn);
    if (flag) {
        ROS_INFO("生成乌龟成功！乌龟的名称为：%s", spawn.response.name.c_str());
    }else {
        ROS_INFO("生成乌龟失败！");
    }

    return 0;
}

修改CMakeLists.txt配置文件：

add_executable(turtle_client_set src/turtle_set.cpp)

target_link_libraries(turtle_client_set
  ${catkin_LIBRARIES}
)

编译整个项目，接着我们来启动服务（提前启动roscore核心和乌龟GUI，可看5.2中的launch启动）：

source ./devel/setup.bash

rosrun communication_practice turtle_client_set

---

5.4、参数设置实战（修改turtle的GUI背景颜色）

目的

目标说明

修改turtlesim乌龟显示节点窗体的背景色，已知背景色是通过参数服务器的方式以 rgb 方式设置的。

确认rosparam及键、格式

获取参数列表：

rosparam list

# 我们可根据对应的参数名成来修改指定的值，下面rgb表示的三元素的值
/turtlesim/background_b
/turtlesim/background_g
/turtlesim/background_r

执行运行节点命令并携带参数：

rosrun turtlesim turtlesim_node _background_r:=37 _background_g=37 _background_b=37

实战服务调用修改背景颜色（C++）

turtle_bgcolor_parmserver.cpp：

#include "ros/ros.h"

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

    //1、初始化ros节点
    ros::init(argc, argv, "setbgcolor_parmserver_node");

    //方式一：NodeHandle实例化设置对象（nh在实例化的时候传入了turtlesim，而下方的setParam设置了background_r，最终设置的属性就是/turtlesim/background_b）
    // ros::NodeHandle nh("turtlesim");
    // nh.setParam("background_r", 200);
    // nh.setParam("background_g", 200);
    // nh.setParam("background_b", 200);

    //方式二：ros::param:set
    ros::param::set("/turtlesim/background_r", 255);
    ros::param::set("/turtlesim/background_g", 255);
    ros::param::set("/turtlesim/background_b", 255);

    return 0;
}

编辑CMakeLists.txt：

add_executable(turtle_bgcolor_parmserver src/turtle_bgcolor_parmserver.cpp)

target_link_libraries(turtle_bgcolor_parmserver
  ${catkin_LIBRARIES}
)

编译整个服务，首先执行roscore服务：

roscore

接着去执行我们编写的服务：

source ./devel/setup.bash

rosrun communication_practice turtle_bgcolor_parmserver

最后启动GUI：

rosrun turtlesim turtlesim_node

其他方式修改parm服务器参数（ros命令、启动节点命令参数、launch+yaml配置）

方式一：通过ros命令来修改param参数服务器中的rgb颜色，重启turtle即可看到效果

rosparam set turtlesim/background_b 155
rosparam set turtlesim/background_g 155
rosparam set turtlesim/background_r 155

方式二：启动节点直接设置参数

rosrun turtlesim turtlesim_node _background_r:=251 _background_g=1 _background_b=25

方式三：

set_bgcolor_launch.launch：

<launch>
    <node pkg="turtlesim" type="turtlesim_node" name="set_bg" output="screen">
        <!-- launch 传参策略1 -->
        <!-- <param name="background_b" value="0" type="int" />
        <param name="background_g" value="0" type="int" />
        <param name="background_r" value="0" type="int" /> -->

        <!-- launch 传参策略2 -->
        <rosparam command="load" file="/home/workspace/roslearn/src/communication_practice/conf/color.yaml" />
    </node>
</launch>

color.yaml：

/turtlesim/background_b: 37
/turtlesim/background_g: 37
/turtlesim/background_r: 37

接着就可以去运行节点服务了：

source ./devel/setup.bash
roslaunch communication_practice set_bgcolor_launch.launch

tips：执行命令启动launch的GUI图背景不会立即改变，需要关闭后重新启动turtlesim_node节点。

参考资料

[1]. ROS与python之时间教程