ROS：通信机制

一、服务通信机制简介

服务通信是ROS中一种极其常用的通信模式，服务通信是基于请求响应模式的，是一种应答机制。也即: 一个节点A向另一个节点B发送请求，B接收处理请求并产生响应结果返回给A。比如如下场景:
机器人巡逻过程中，控制系统分析传感器数据发现可疑物体或人… 此时需要拍摄照片并留存。

在上述场景中，就使用到了服务通信。
一个节点需要向相机节点发送拍照请求，相机节点处理请求，并返回处理结果

与上述应用类似的，服务通信更适用于对时时性有要求、具有一定逻辑处理的应用场景。

1.1概念

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

1.2作用

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

二、服务通信理论模型

服务是一种带有应答的通信机制，与话题的通信相比，其减少了Listener与Talker之间的RPC通信。
服务通信较之于话题通信更简单些，理论模型如下图所示，该模型中涉及到三个角色:

ROS master(管理者)
Server(服务端)
Client(客户端)
ROS Master 负责保管 Server 和 Client 注册的信息，并匹配话题相同的 Server 与 Client ，帮助 Server 与 Client 建立连接，连接建立后，Client 发送请求信息，Server 返回响应信息。

0、Talker注册
Talker启动，通过1234端口使用RPC向ROS Master注册发布者的信息，包含所提供的服务名;ROS Master会将节点的注册信息加入注册列表中。
1、Listener注册
Listener启动，同样通过RPC向ROS Master注册订阅者的信息，包含需要订阅的服务名。
2、ROS Master进行信息匹配
Master根据Listener的订阅信息从注册列表中进行查找，如果没有找到匹配的服务提供者，则等待该服务的提供者加入：如果找到匹配的服务提供者信息，则通过RPC向Listener发送Talker的TCP地址信息。
4、Listener与Talker建立网络连接
Listener接收到确认信息后，使用TCP尝试与Talker建立网络连接，并且发送服务的请求数据。
5、Talker向Listener发布服务应答数据
Talker接收到服务请求和参数后，开始执行服务功能，执行完成后，向Listener发送应答数据。

三、服务通信自定义srv

3.1目的

服务通信中，客户端提交两个整数至服务端，服务端求和并响应结果到客户端，请创建服务器与客户端通信的数据载体。

3.2流程

srv 文件内的可用数据类型与 msg 文件一致，且定义 srv 实现流程与自定义 msg 实现流程类似:

按照固定格式创建srv文件
编辑配置文件
编译生成中间文件

3.2.1创建srv文件

服务通信中，数据分成两部分，请求与响应，在 srv 文件中请求和响应使用—分割，具体实现如下:
功能包下新建 srv 目录，添加 xxx.srv 文件，内容:

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

3.2.2编辑配置文件

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

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

CMakeLists.txt编辑 srv 相关配置

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

add_service_files(
  FILES
  AddInts.srv
)

generate_messages(
  DEPENDENCIES
  std_msgs
)

3.2.3.编译

编译后的中间文件查看:

C++ 需要调用的中间文件(…/工作空间/devel/include/包名/xxx.h)

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

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

四、服务通信自定义srv调用(C++)

4.1要求

编写服务通信，客户端提交两个整数至服务端，服务端求和并响应结果到客户端。

4.2流程

分析:
在模型实现中，ROS master 不需要实现，而连接的建立也已经被封装了，需要关注的关键点有三个:
服务端
客户端
数据

流程:
编写服务端实现；
编写客户端实现；
编辑配置文件；
编译并执行。

4.3vscode配置

需要像之前自定义 msg 实现一样配置c_cpp_properies.json 文件，如果以前已经配置且没有变更工作空间，可以忽略，如果需要配置，配置方式与之前相同:

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

4.4服务端代码

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

/*
服务端实现：解析客户端提交的数据，并运算产生响应
1.包含头文件
2.初始化ros节点
3.创建节点句柄
4.创建一个服务对象
5.处理请求并产生响应
6.spin()
*/

bool doNums(plumbing_server_client::AddInts::Request & request,
            plumbing_server_client::AddInts::Response & response)
{
    //1.处理请求
    int num1 = request.num1;
    int num2 = request.num2;
    ROS_INFO("收到的请求数据是:num1=%d,num2=%d",num1,num2);
   
    //2.组织响应
    int sum=num1+num2;
    response.sum = sum;
    ROS_INFO("求和的结果： sum= %d",sum);
    return true;  
}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化ros节点
    ros::init(argc,argv,"heishui");//节点名称需保证唯一
    // 3.创建节点句柄
    ros::NodeHandle nh;
    // 4.创建一个服务对象
    ros::ServiceServer serve = nh.advertiseService("addInts",doNums);
    ROS_INFO("服务端启动");
    // 5.处理请求并产生响应
    // 6.spin()
    ros::spin();



    return 0;
}

4.5客户端代码

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

/*
客户端实现：提交两个整数，并处理响应的结果
1.包含头文件
2.初始化ros节点
3.创建节点句柄
4.创建一个客户端对象
5.请求请求并产生响应，并组织进 request

实现参数的动态提交
1.格式 rosrun xxxx xxxx 12 34
2.节点执行时，需要获取命令中的参数，
*/


int main(int argc, char *argv[])
{
    //获取命令中的参数
    if(argc !=3)
    {
        ROS_INFO("提交的参数个数不对");
        return 1;
    }
    


    setlocale(LC_ALL,"");
    // 2.初始化ros节点
    ros::init(argc,argv,"dabao");//节点名称需保证唯一
    // 3.创建节点句柄
    ros::NodeHandle nh;
    // 4.创建一个客户端对象
    ros::ServiceClient client=nh.serviceClient<plumbing_server_client::AddInts>("addInts");
    ROS_INFO("客户端启动");
    // 5.处理请求并产生响应
    plumbing_server_client::AddInts ai;
    //5.1组织请求
    //规定数字
    // ai.request.num1=100;
    // ai.request.num2=200;
    //获取命令中的参数
    ai.request.num1=atoi(argv[1]);
    ai.request.num2=atoi(argv[2]);

    //5.2处理响应
    //一般操作是先启动服务端，在启动客户端，否则报错
    //可以先将客户端挂起。ros中内置了相关函数，这些函数可以让客户端启动后挂起，等待服务器启动
    //调用判断服务器状态的函数
    //函数1
    //client.waitForExistence();
    //函数2
    ros::service::waitForService("addInts");

    bool flag =client.call(ai);
    if(flag)
    {
        ROS_INFO("响应成功");
        //获取结果
        ROS_INFO("响应结果sum= %d",ai.response.sum);
    }
    else
    {
        ROS_INFO("处理失败");
    }



    return 0;
}

4.6配置 CMakeLists.txt

## The recommended prefix ensures that target names across packages don't collide
add_executable(demo01_server src/demo01_server.cpp)
add_executable(demo02_client src/demo02_client.cpp)

## Add cmake target dependencies of the executable
## same as for the library above
add_dependencies(demo01_server ${PROJECT_NAME}_gencpp)
add_dependencies(demo02_client ${PROJECT_NAME}_gencpp)

## Specify libraries to link a library or executable target against
target_link_libraries(demo01_server
  ${catkin_LIBRARIES}
)
target_link_libraries(demo02_client
  ${catkin_LIBRARIES}
)

4.7执行

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

注意:
如果先启动客户端，那么会导致运行失败
优化:
在客户端发送请求前添加:client.waitForExistence();
或:ros::service::waitForService(“AddInts”);
这是一个阻塞式函数，只有服务启动成功后才会继续执行
此处可以使用 launch 文件优化，但是需要注意 args 传参特点

五、服务通信自定义srv调用(python)

5.1需求

编写服务通信，客户端提交两个整数至服务端，服务端求和并响应结果到客户端。

5.2流程

分析:
在模型实现中，ROS master 不需要实现，而连接的建立也已经被封装了，需要关注的关键点有三个:
服务端
客户端
数据

流程:
编写服务端实现；
编写客户端实现；
为python文件添加可执行权限；
编辑配置文件；
编译并执行。

5.3vscode配置

需要像之前自定义 msg 实现一样配置settings.json 文件，如果以前已经配置且没有变更工作空间，可以忽略，如果需要配置，配置方式与之前相同:

{
    "python.autoComplete.extraPaths": [
        "/opt/ros/noetic/lib/python3/dist-packages",
    ]
}

5.4服务端代码

! /usr/bin/env python

import rospy
# from plumbing_server_client.srv import  AddInts,AddIntsRequest,AddIntsRespones
from plumbing_server_client.srv import *

"""
    服务端：解析客户端请求，产生响应
    1.导包
    2.初始化ros节点s
    3.创建服务端对象
    4.处理逻辑（回调函数）
    5spin()
"""

#参数：封装了请求数据
#返回值：响应数据
def doNum(request):
    #1.解析提交的两个整数
    num1=request.num1
    num2=request.num2
    #2.求和
    sum=num1+num2
    #3.将结果封装进响应
    response = AddIntsResponse()
    response.sum=sum

    rospy.loginfo("服务器解析数据num1= %d, num2= %d,响应的结果:sum= %d",num1,num2,sum)

    return response

if __name__ == "__main__":
    # 2.初始化ros节点
    rospy.init_node("heishui")
    # 3.创建服务端对象
    server = rospy.Service("addInts",AddInts,doNum)
    rospy.loginfo("服务器已经启动")
    # 4.处理逻辑（回调函数）
    # 5spin()
    rospy.spin()

5.5客户端代码

#! /usr/bin/env python

import rospy
# from plumbing_server_client.srv import  AddInts,AddIntsRequest,AddIntsRespones
from plumbing_server_client.srv import *
import sys

"""
    客户端：组织并提交请求，处理服务端响应
    1.导包
    2.初始化ros节点s
    3.创建客户端对象
    4.组织请求数据并发送请求
    5.处理响应
"""


if __name__ == "__main__":
    #判断参数长度
    if len(sys.argv)!=3:
        rospy.logerr("传入参数个数不对")
        sys.exit(1)

    # 2.初始化ros节点
    rospy.init_node("erhei")
    # 3.创建服务端对象
    client = rospy.ServiceProxy("addInts",AddInts)
    rospy.loginfo("客户端已经启动")
    # 4.组织请求数据并发送请求
    #固定数字
    # response=client.call(12,34)
    #捕捉传入的参数
    num1=int(sys.argv[1])
    num2=int(sys.argv[2])

    # 一般操作是先启动服务端，在启动客户端，否则报错
    # 可以先将客户端挂起。ros中内置了相关函数，这些函数可以让客户端启动后挂起，等待服务器启动
    # 调用判断服务器状态的函数
    # 函数1
    # client.wait_for_service()
    #函数2
    rospy.wait_for_service("addInts")

    response=client.call(num1,num2)
    #5.处理响应
    rospy.loginfo("响应的数据：%d",response.sum)

5.6设置权限

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

5.7配置 CMakeLists.txt

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

5.8执行

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

结果：